244 lines
8.8 KiB
C++
244 lines
8.8 KiB
C++
#include "path_tracker.h"
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
#include <cmath>
|
|
|
|
/**
|
|
* @brief Simple console GUI display class
|
|
* Display control values in table format in terminal
|
|
*/
|
|
class ConsoleGUI {
|
|
public:
|
|
ConsoleGUI() = default;
|
|
|
|
/**
|
|
* @brief Clear screen
|
|
*/
|
|
void clear() {
|
|
#ifdef _WIN32
|
|
system("cls");
|
|
#else
|
|
system("clear");
|
|
#endif
|
|
}
|
|
|
|
/**
|
|
* @brief Show title
|
|
*/
|
|
void showTitle() {
|
|
std::cout << "\n";
|
|
std::cout << "========================================================================\n";
|
|
std::cout << "| Single Steering Wheel AGV Path Tracking Control System - GUI |\n";
|
|
std::cout << "========================================================================\n";
|
|
std::cout << "\n";
|
|
}
|
|
|
|
/**
|
|
* @brief Show AGV parameters
|
|
*/
|
|
void showAGVParams(const AGVModel& model) {
|
|
std::cout << "+---------------------- AGV Parameters ----------------------+\n";
|
|
std::cout << "| Wheelbase: " << std::setw(8) << std::fixed << std::setprecision(2)
|
|
<< model.getWheelbase() << " m |\n";
|
|
std::cout << "| Max Velocity: " << std::setw(8) << model.getMaxVelocity() << " m/s |\n";
|
|
std::cout << "| Max Steering Angle: " << std::setw(8) << (model.getMaxSteeringAngle() * 180.0 / M_PI)
|
|
<< " degrees |\n";
|
|
std::cout << "+------------------------------------------------------------+\n\n";
|
|
}
|
|
|
|
/**
|
|
* @brief Show control sequence table
|
|
*/
|
|
void showControlTable(const ControlSequence& sequence, int max_rows = 20) {
|
|
if (sequence.size() == 0) {
|
|
std::cout << "Control sequence is empty!\n";
|
|
return;
|
|
}
|
|
|
|
std::cout << "+---------------- Control Sequence ----------------+\n";
|
|
std::cout << "| " << std::setw(8) << "Step"
|
|
<< " | " << std::setw(8) << "Time(s)"
|
|
<< " | " << std::setw(10) << "Velocity(m/s)"
|
|
<< " | " << std::setw(12) << "Steering(deg)"
|
|
<< " |\n";
|
|
std::cout << "+----------+----------+------------+---------------+\n";
|
|
|
|
int display_rows = std::min(max_rows, static_cast<int>(sequence.size()));
|
|
int step = std::max(1, static_cast<int>(sequence.size()) / display_rows);
|
|
|
|
for (size_t i = 0; i < sequence.size(); i += step) {
|
|
if (i / step >= display_rows) break;
|
|
|
|
double time = sequence.timestamps[i];
|
|
double velocity = sequence.controls[i].v;
|
|
double steering_deg = sequence.controls[i].delta * 180.0 / M_PI;
|
|
|
|
std::cout << "| " << std::setw(8) << i
|
|
<< " | " << std::setw(8) << std::fixed << std::setprecision(2) << time
|
|
<< " | " << std::setw(12) << std::setprecision(4) << velocity
|
|
<< " | " << std::setw(13) << std::setprecision(4) << steering_deg
|
|
<< " |\n";
|
|
}
|
|
|
|
std::cout << "+----------+----------+------------+---------------+\n";
|
|
std::cout << "Total steps: " << sequence.size() << "\n\n";
|
|
}
|
|
|
|
/**
|
|
* @brief Show current state and control (dashboard style)
|
|
*/
|
|
void showDashboard(const AGVModel::State& state, const AGVModel::Control& control, int step) {
|
|
std::cout << "+------------ Current State (Step: " << std::setw(4) << step << ") ------------+\n";
|
|
|
|
// Position
|
|
std::cout << "| |\n";
|
|
std::cout << "| Position: X = " << std::setw(8) << std::fixed << std::setprecision(3)
|
|
<< state.x << " m Y = " << std::setw(8) << state.y << " m |\n";
|
|
|
|
// Heading
|
|
double theta_deg = state.theta * 180.0 / M_PI;
|
|
std::cout << "| Heading: theta = " << std::setw(8) << std::setprecision(2)
|
|
<< theta_deg << " degrees |\n";
|
|
|
|
std::cout << "| |\n";
|
|
std::cout << "+------------------------------------------------------------+\n";
|
|
std::cout << "| Control Values |\n";
|
|
std::cout << "+------------------------------------------------------------+\n";
|
|
|
|
// Velocity bar
|
|
std::cout << "| Velocity: " << std::setw(6) << std::setprecision(3) << control.v << " m/s ";
|
|
drawBar(control.v, 0, 2.0, 20);
|
|
std::cout << " |\n";
|
|
|
|
// Steering bar
|
|
double delta_deg = control.delta * 180.0 / M_PI;
|
|
std::cout << "| Steering: " << std::setw(6) << std::setprecision(2) << delta_deg << " deg ";
|
|
drawBar(delta_deg, -45, 45, 20);
|
|
std::cout << " |\n";
|
|
|
|
std::cout << "+------------------------------------------------------------+\n\n";
|
|
}
|
|
|
|
/**
|
|
* @brief Show statistics
|
|
*/
|
|
void showStatistics(const ControlSequence& sequence) {
|
|
if (sequence.size() == 0) return;
|
|
|
|
// Calculate statistics
|
|
double avg_velocity = 0.0;
|
|
double max_velocity = -1e9;
|
|
double min_velocity = 1e9;
|
|
double avg_steering = 0.0;
|
|
double max_steering = -1e9;
|
|
double min_steering = 1e9;
|
|
|
|
for (const auto& ctrl : sequence.controls) {
|
|
avg_velocity += ctrl.v;
|
|
max_velocity = std::max(max_velocity, ctrl.v);
|
|
min_velocity = std::min(min_velocity, ctrl.v);
|
|
|
|
double delta_deg = ctrl.delta * 180.0 / M_PI;
|
|
avg_steering += delta_deg;
|
|
max_steering = std::max(max_steering, delta_deg);
|
|
min_steering = std::min(min_steering, delta_deg);
|
|
}
|
|
|
|
avg_velocity /= sequence.size();
|
|
avg_steering /= sequence.size();
|
|
|
|
std::cout << "+---------------- Statistics ----------------+\n";
|
|
std::cout << "| Velocity Statistics: |\n";
|
|
std::cout << "| Average: " << std::setw(8) << std::fixed << std::setprecision(4)
|
|
<< avg_velocity << " m/s |\n";
|
|
std::cout << "| Maximum: " << std::setw(8) << max_velocity << " m/s |\n";
|
|
std::cout << "| Minimum: " << std::setw(8) << min_velocity << " m/s |\n";
|
|
std::cout << "| |\n";
|
|
std::cout << "| Steering Angle Statistics: |\n";
|
|
std::cout << "| Average: " << std::setw(8) << std::setprecision(4)
|
|
<< avg_steering << " degrees |\n";
|
|
std::cout << "| Maximum: " << std::setw(8) << max_steering << " degrees |\n";
|
|
std::cout << "| Minimum: " << std::setw(8) << min_steering << " degrees |\n";
|
|
std::cout << "+--------------------------------------------+\n";
|
|
}
|
|
|
|
private:
|
|
/**
|
|
* @brief Draw bar chart
|
|
*/
|
|
void drawBar(double value, double min_val, double max_val, int width) {
|
|
double normalized = (value - min_val) / (max_val - min_val);
|
|
normalized = std::max(0.0, std::min(1.0, normalized));
|
|
|
|
int filled = static_cast<int>(normalized * width);
|
|
|
|
std::cout << "[";
|
|
for (int i = 0; i < width; ++i) {
|
|
if (i < filled) {
|
|
std::cout << "=";
|
|
} else {
|
|
std::cout << " ";
|
|
}
|
|
}
|
|
std::cout << "]";
|
|
}
|
|
};
|
|
|
|
|
|
int main() {
|
|
ConsoleGUI gui;
|
|
|
|
// Create AGV model
|
|
AGVModel agv_model(1.0, 2.0, M_PI / 4);
|
|
|
|
// Create path tracker
|
|
PathTracker tracker(agv_model);
|
|
|
|
// Create circular arc path as example
|
|
PathCurve path;
|
|
path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI / 2, 100);
|
|
tracker.setReferencePath(path);
|
|
|
|
// Set initial state
|
|
AGVModel::State initial_state(0.0, 0.0, 0.0);
|
|
tracker.setInitialState(initial_state);
|
|
|
|
// Generate control sequence
|
|
tracker.generateControlSequence("pure_pursuit", 0.1, 10.0);
|
|
const ControlSequence& sequence = tracker.getControlSequence();
|
|
|
|
// Show interface
|
|
gui.clear();
|
|
gui.showTitle();
|
|
gui.showAGVParams(agv_model);
|
|
gui.showControlTable(sequence, 15);
|
|
gui.showStatistics(sequence);
|
|
|
|
// Real-time simulation display (optional)
|
|
std::cout << "\nPlay real-time simulation? (y/n): ";
|
|
char choice;
|
|
std::cin >> choice;
|
|
|
|
if (choice == 'y' || choice == 'Y') {
|
|
for (size_t i = 0; i < sequence.size(); i += 5) { // Display every 5 steps
|
|
gui.clear();
|
|
gui.showTitle();
|
|
gui.showDashboard(sequence.predicted_states[i], sequence.controls[i], i);
|
|
|
|
// Pause for viewing
|
|
#ifdef _WIN32
|
|
system("timeout /t 1 /nobreak > nul");
|
|
#else
|
|
system("sleep 0.5");
|
|
#endif
|
|
}
|
|
}
|
|
|
|
std::cout << "\nProgram ended. Press Enter to exit...";
|
|
std::cin.ignore();
|
|
std::cin.get();
|
|
|
|
return 0;
|
|
}
|