initial
This commit is contained in:
CaiXiang
243
examples/gui_demo.cpp
Normal file
243
examples/gui_demo.cpp
Normal file
@@ -0,0 +1,243 @@
|
||||
#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;
|
||||
}
|
||||
Reference in New Issue
Block a user