652 lines
22 KiB
C++
652 lines
22 KiB
C++
#include "path_tracker.h"
|
||
#include <QApplication>
|
||
#include <QMainWindow>
|
||
#include <QWidget>
|
||
#include <QPushButton>
|
||
#include <QVBoxLayout>
|
||
#include <QHBoxLayout>
|
||
#include <QLabel>
|
||
#include <QComboBox>
|
||
#include <QDoubleSpinBox>
|
||
#include <QTableWidget>
|
||
#include <QGroupBox>
|
||
#include <QPainter>
|
||
#include <QTimer>
|
||
#include <QHeaderView>
|
||
#include <cmath>
|
||
|
||
#include <QFileDialog>
|
||
#include <QMessageBox>
|
||
#include <QInputDialog>
|
||
|
||
/**
|
||
* @brief AGV Path Visualization Widget
|
||
* Displays the reference path and AGV trajectory
|
||
*/
|
||
class PathVisualizationWidget : public QWidget {
|
||
Q_OBJECT
|
||
|
||
public:
|
||
explicit PathVisualizationWidget(QWidget* parent = nullptr)
|
||
: QWidget(parent), current_step_(0), show_animation_(false) {
|
||
setMinimumSize(600, 600);
|
||
setStyleSheet("background-color: white;");
|
||
}
|
||
|
||
void setPath(const PathCurve& path) {
|
||
path_ = path;
|
||
update();
|
||
}
|
||
|
||
void setControlSequence(const ControlSequence& sequence) {
|
||
sequence_ = sequence;
|
||
current_step_ = 0;
|
||
update();
|
||
}
|
||
|
||
void setCurrentStep(int step) {
|
||
current_step_ = step;
|
||
update();
|
||
}
|
||
|
||
void setShowAnimation(bool show) {
|
||
show_animation_ = show;
|
||
update();
|
||
}
|
||
|
||
protected:
|
||
void paintEvent(QPaintEvent* event) override {
|
||
QPainter painter(this);
|
||
painter.setRenderHint(QPainter::Antialiasing);
|
||
|
||
// Calculate coordinate transformation
|
||
const auto& path_points = path_.getPathPoints();
|
||
if (path_points.empty()) return;
|
||
|
||
// Find bounds
|
||
double min_x = 1e9, max_x = -1e9, min_y = 1e9, max_y = -1e9;
|
||
for (const auto& pt : path_points) {
|
||
min_x = std::min(min_x, pt.x);
|
||
max_x = std::max(max_x, pt.x);
|
||
min_y = std::min(min_y, pt.y);
|
||
max_y = std::max(max_y, pt.y);
|
||
}
|
||
|
||
// Add trajectory points
|
||
if (!sequence_.predicted_states.empty()) {
|
||
for (const auto& state : sequence_.predicted_states) {
|
||
min_x = std::min(min_x, state.x);
|
||
max_x = std::max(max_x, state.x);
|
||
min_y = std::min(min_y, state.y);
|
||
max_y = std::max(max_y, state.y);
|
||
}
|
||
}
|
||
|
||
// Add margin
|
||
double margin = 0.5;
|
||
min_x -= margin; max_x += margin;
|
||
min_y -= margin; max_y += margin;
|
||
|
||
double range_x = max_x - min_x;
|
||
double range_y = max_y - min_y;
|
||
double range = std::max(range_x, range_y);
|
||
|
||
// Center the view
|
||
double center_x = (min_x + max_x) / 2.0;
|
||
double center_y = (min_y + max_y) / 2.0;
|
||
|
||
// Scale to fit widget with padding
|
||
int padding = 40;
|
||
// Prevent division by zero if all points are at the same location
|
||
if (range < 1e-6) {
|
||
range = 1.0;
|
||
}
|
||
double scale = std::min(width() - 2 * padding, height() - 2 * padding) / range;
|
||
|
||
// Coordinate transformation: world to screen
|
||
auto toScreen = [&](double x, double y) -> QPointF {
|
||
double sx = (x - center_x) * scale + width() / 2.0;
|
||
double sy = height() / 2.0 - (y - center_y) * scale; // Flip Y axis
|
||
return QPointF(sx, sy);
|
||
};
|
||
|
||
// Draw grid
|
||
painter.setPen(QPen(QColor(220, 220, 220), 1));
|
||
int grid_lines = 10;
|
||
for (int i = 0; i <= grid_lines; ++i) {
|
||
double t = static_cast<double>(i) / grid_lines;
|
||
double x = min_x + t * range;
|
||
double y = min_y + t * range;
|
||
|
||
QPointF p1 = toScreen(x, min_y);
|
||
QPointF p2 = toScreen(x, min_y + range);
|
||
painter.drawLine(p1, p2);
|
||
|
||
p1 = toScreen(min_x, y);
|
||
p2 = toScreen(min_x + range, y);
|
||
painter.drawLine(p1, p2);
|
||
}
|
||
|
||
// Draw axes
|
||
painter.setPen(QPen(Qt::black, 2));
|
||
QPointF origin = toScreen(0, 0);
|
||
QPointF x_axis = toScreen(1, 0);
|
||
QPointF y_axis = toScreen(0, 1);
|
||
|
||
painter.drawLine(origin, x_axis);
|
||
painter.drawLine(origin, y_axis);
|
||
painter.drawText(x_axis + QPointF(5, 5), "X");
|
||
painter.drawText(y_axis + QPointF(5, 5), "Y");
|
||
|
||
// Draw reference path
|
||
painter.setPen(QPen(QColor(100, 100, 255), 3, Qt::DashLine));
|
||
for (size_t i = 1; i < path_points.size(); ++i) {
|
||
QPointF p1 = toScreen(path_points[i-1].x, path_points[i-1].y);
|
||
QPointF p2 = toScreen(path_points[i].x, path_points[i].y);
|
||
painter.drawLine(p1, p2);
|
||
}
|
||
|
||
// Draw path points
|
||
painter.setPen(Qt::NoPen);
|
||
painter.setBrush(QColor(100, 100, 255, 100));
|
||
for (const auto& pt : path_points) {
|
||
QPointF p = toScreen(pt.x, pt.y);
|
||
painter.drawEllipse(p, 3, 3);
|
||
}
|
||
|
||
// Draw predicted trajectory
|
||
if (!sequence_.predicted_states.empty()) {
|
||
painter.setPen(QPen(QColor(255, 100, 100), 2));
|
||
for (size_t i = 1; i < sequence_.predicted_states.size(); ++i) {
|
||
QPointF p1 = toScreen(sequence_.predicted_states[i-1].x,
|
||
sequence_.predicted_states[i-1].y);
|
||
QPointF p2 = toScreen(sequence_.predicted_states[i].x,
|
||
sequence_.predicted_states[i].y);
|
||
painter.drawLine(p1, p2);
|
||
}
|
||
|
||
// Draw current AGV position
|
||
if (show_animation_ && current_step_ < sequence_.predicted_states.size()) {
|
||
const auto& state = sequence_.predicted_states[current_step_];
|
||
QPointF pos = toScreen(state.x, state.y);
|
||
|
||
// Draw AGV body (rectangle)
|
||
painter.save();
|
||
painter.translate(pos);
|
||
painter.rotate(-state.theta * 180.0 / M_PI); // Rotate to heading
|
||
|
||
double agv_length = 0.3 * scale;
|
||
double agv_width = 0.2 * scale;
|
||
|
||
painter.setBrush(QColor(50, 200, 50));
|
||
painter.setPen(QPen(Qt::black, 2));
|
||
painter.drawRect(QRectF(-agv_length/2, -agv_width/2,
|
||
agv_length, agv_width));
|
||
|
||
// Draw heading indicator (arrow)
|
||
painter.setBrush(QColor(255, 50, 50));
|
||
painter.drawEllipse(QPointF(agv_length/2, 0),
|
||
agv_width/4, agv_width/4);
|
||
|
||
painter.restore();
|
||
|
||
// Draw position label
|
||
painter.setPen(Qt::black);
|
||
painter.drawText(pos + QPointF(10, -10),
|
||
QString("Step: %1").arg(current_step_));
|
||
}
|
||
}
|
||
|
||
// Draw legend
|
||
int legend_x = 10;
|
||
int legend_y = 10;
|
||
painter.fillRect(legend_x, legend_y, 150, 80, QColor(255, 255, 255, 200));
|
||
painter.setPen(Qt::black);
|
||
painter.drawRect(legend_x, legend_y, 150, 80);
|
||
|
||
// Reference path
|
||
painter.setPen(QPen(QColor(100, 100, 255), 3, Qt::DashLine));
|
||
painter.drawLine(legend_x + 10, legend_y + 20, legend_x + 40, legend_y + 20);
|
||
painter.setPen(Qt::black);
|
||
painter.drawText(legend_x + 50, legend_y + 25, "Reference Path");
|
||
|
||
// Trajectory
|
||
painter.setPen(QPen(QColor(255, 100, 100), 3));
|
||
painter.drawLine(legend_x + 10, legend_y + 40, legend_x + 40, legend_y + 40);
|
||
painter.setPen(Qt::black);
|
||
painter.drawText(legend_x + 50, legend_y + 45, "Trajectory");
|
||
|
||
// AGV
|
||
painter.fillRect(legend_x + 10, legend_y + 55, 30, 15, QColor(50, 200, 50));
|
||
painter.drawRect(legend_x + 10, legend_y + 55, 30, 15);
|
||
painter.drawText(legend_x + 50, legend_y + 65, "AGV");
|
||
}
|
||
|
||
private:
|
||
PathCurve path_;
|
||
ControlSequence sequence_;
|
||
int current_step_;
|
||
bool show_animation_;
|
||
};
|
||
|
||
/**
|
||
* @brief Main Window for AGV Path Tracking GUI
|
||
*/
|
||
class MainWindow : public QMainWindow {
|
||
Q_OBJECT
|
||
|
||
public:
|
||
MainWindow(QWidget* parent = nullptr) : QMainWindow(parent) {
|
||
setWindowTitle("AGV Path Tracking Control System - Qt GUI");
|
||
resize(1200, 800);
|
||
|
||
// Create central widget
|
||
QWidget* central = new QWidget(this);
|
||
setCentralWidget(central);
|
||
|
||
QHBoxLayout* main_layout = new QHBoxLayout(central);
|
||
|
||
// Left panel: Visualization
|
||
visualization_ = new PathVisualizationWidget(this);
|
||
main_layout->addWidget(visualization_, 2);
|
||
|
||
// Right panel: Controls and data
|
||
QWidget* right_panel = new QWidget(this);
|
||
QVBoxLayout* right_layout = new QVBoxLayout(right_panel);
|
||
|
||
// AGV Parameters Group
|
||
QGroupBox* param_group = new QGroupBox("AGV Parameters", this);
|
||
QVBoxLayout* param_layout = new QVBoxLayout(param_group);
|
||
|
||
wheelbase_spin_ = createParamRow("Wheelbase (m):", 0.5, 3.0, 1.0, param_layout);
|
||
max_vel_spin_ = createParamRow("Max Velocity (m/s):", 0.5, 5.0, 2.0, param_layout);
|
||
max_steer_spin_ = createParamRow("Max Steering (deg):", 10, 60, 45, param_layout);
|
||
|
||
right_layout->addWidget(param_group);
|
||
|
||
// Control Parameters Group
|
||
QGroupBox* control_group = new QGroupBox("Control Parameters", this);
|
||
QVBoxLayout* control_layout = new QVBoxLayout(control_group);
|
||
|
||
// Algorithm selection
|
||
QHBoxLayout* algo_layout = new QHBoxLayout();
|
||
algo_layout->addWidget(new QLabel("Algorithm:", this));
|
||
algorithm_combo_ = new QComboBox(this);
|
||
algorithm_combo_->addItem("Pure Pursuit");
|
||
algorithm_combo_->addItem("Stanley");
|
||
algo_layout->addWidget(algorithm_combo_);
|
||
control_layout->addLayout(algo_layout);
|
||
// Path type selection
|
||
QHBoxLayout* path_layout = new QHBoxLayout();
|
||
path_layout->addWidget(new QLabel("Path Type:", this));
|
||
path_combo_ = new QComboBox(this);
|
||
path_combo_->addItem("Circle Arc");
|
||
path_combo_->addItem("Straight Line");
|
||
path_combo_->addItem("S-Curve");
|
||
//自定义路径
|
||
path_combo_->addItem("Load from CSV");
|
||
path_combo_->addItem("Custom Spline");
|
||
|
||
path_layout->addWidget(path_combo_);
|
||
control_layout->addLayout(path_layout);
|
||
|
||
dt_spin_ = createParamRow("Time Step (s):", 0.01, 1.0, 0.1, control_layout);
|
||
horizon_spin_ = createParamRow("Horizon (s):", 1.0, 100.0, 50.0, control_layout);
|
||
|
||
right_layout->addWidget(control_group);
|
||
|
||
control_layout->addLayout(path_layout);
|
||
|
||
// 添加自定义路径按钮
|
||
QHBoxLayout* custom_btn_layout = new QHBoxLayout();
|
||
|
||
|
||
QPushButton* load_csv_btn = new QPushButton("Browse CSV...", this);
|
||
connect(load_csv_btn, &QPushButton::clicked, [this]() {
|
||
QString filename = QFileDialog::getOpenFileName(
|
||
this, "Open CSV Path File", "", "CSV Files (*.csv)");
|
||
if (!filename.isEmpty()) {
|
||
// 修复: 使用toLocal8Bit以正确处理Windows路径（包括中文路径）
|
||
if (custom_path_.loadFromCSV(filename.toLocal8Bit().constData(), true)) {
|
||
custom_path_loaded_ = true;
|
||
QMessageBox::information(this, "Success",
|
||
QString("Loaded %1 points from CSV!").arg(
|
||
custom_path_.getPathPoints().size()));
|
||
} else {
|
||
QMessageBox::warning(this, "Error", "Failed to load CSV file!");
|
||
}
|
||
}
|
||
});
|
||
custom_btn_layout->addWidget(load_csv_btn);
|
||
|
||
QPushButton* save_csv_btn = new QPushButton("Save Path...", this);
|
||
connect(save_csv_btn, &QPushButton::clicked, [this]() {
|
||
QString filename = QFileDialog::getSaveFileName(
|
||
this, "Save Path as CSV", "my_path.csv", "CSV Files (*.csv)");
|
||
// 修复: 使用toLocal8Bit以正确处理Windows路径（包括中文路径）
|
||
if (!filename.isEmpty() && custom_path_loaded_) {
|
||
if (custom_path_.saveToCSV(filename.toLocal8Bit().constData())) {
|
||
QMessageBox::information(this, "Success", "Path saved!");
|
||
}
|
||
}
|
||
});
|
||
custom_btn_layout->addWidget(save_csv_btn);
|
||
|
||
control_layout->addLayout(custom_btn_layout);
|
||
|
||
|
||
|
||
// Buttons
|
||
QHBoxLayout* button_layout = new QHBoxLayout();
|
||
|
||
QPushButton* generate_btn = new QPushButton("Generate Control", this);
|
||
connect(generate_btn, &QPushButton::clicked, this, &MainWindow::generateControl);
|
||
button_layout->addWidget(generate_btn);
|
||
|
||
start_btn_ = new QPushButton("Start Animation", this);
|
||
connect(start_btn_, &QPushButton::clicked, this, &MainWindow::toggleAnimation);
|
||
start_btn_->setEnabled(false);
|
||
button_layout->addWidget(start_btn_);
|
||
|
||
right_layout->addLayout(button_layout);
|
||
|
||
// Statistics Group
|
||
stats_group_ = new QGroupBox("Statistics", this);
|
||
QVBoxLayout* stats_layout = new QVBoxLayout(stats_group_);
|
||
|
||
stats_label_ = new QLabel("No data", this);
|
||
stats_label_->setWordWrap(true);
|
||
stats_layout->addWidget(stats_label_);
|
||
|
||
right_layout->addWidget(stats_group_);
|
||
|
||
// Control Sequence Table
|
||
table_ = new QTableWidget(this);
|
||
table_->setColumnCount(4);
|
||
table_->setHorizontalHeaderLabels({"Step", "Time(s)", "Velocity(m/s)", "Steering(deg)"});
|
||
table_->horizontalHeader()->setStretchLastSection(true);
|
||
table_->setAlternatingRowColors(true);
|
||
right_layout->addWidget(table_);
|
||
|
||
main_layout->addWidget(right_panel, 1);
|
||
|
||
// Initialize AGV model
|
||
updateAGVModel();
|
||
|
||
// Animation timer
|
||
animation_timer_ = new QTimer(this);
|
||
connect(animation_timer_, &QTimer::timeout, this, &MainWindow::updateAnimation);
|
||
}
|
||
|
||
private slots:
|
||
void generateControl() {
|
||
// Update AGV model
|
||
updateAGVModel();
|
||
|
||
// Create path based on selection
|
||
PathCurve path;
|
||
QString path_type = path_combo_->currentText();
|
||
|
||
if (path_type == "Load from CSV") {
|
||
if (!custom_path_loaded_) {
|
||
QMessageBox::warning(this, "Warning",
|
||
"Please load a CSV file first using 'Browse CSV...' button!");
|
||
return;
|
||
}
|
||
path = custom_path_;
|
||
}
|
||
else if (path_type == "Custom Spline") {
|
||
if (!custom_path_loaded_) {
|
||
// 如果没有预加载，让用户输入关键点
|
||
bool ok;
|
||
int num_points = QInputDialog::getInt(this, "Spline Input",
|
||
"Number of key points (2-10):", 4, 2, 10, 1, &ok);
|
||
if (!ok) return;
|
||
|
||
std::vector<PathPoint> key_points;
|
||
for (int i = 0; i < num_points; ++i) {
|
||
double x = QInputDialog::getDouble(this, "Key Point",
|
||
QString("Point %1 - X coordinate:").arg(i+1),
|
||
i * 3.0, -100, 100, 2, &ok);
|
||
if (!ok) return;
|
||
|
||
double y = QInputDialog::getDouble(this, "Key Point",
|
||
QString("Point %1 - Y coordinate:").arg(i+1),
|
||
(i % 2) * 3.0, -100, 100, 2, &ok);
|
||
if (!ok) return;
|
||
|
||
key_points.push_back(PathPoint(x, y));
|
||
}
|
||
|
||
path.generateSpline(key_points, 200, 0.5);
|
||
custom_path_ = path;
|
||
custom_path_loaded_ = true;
|
||
} else {
|
||
path = custom_path_;
|
||
}
|
||
}
|
||
else if (path_type == "Circle Arc") {
|
||
path.generateCircleArc(5.0, 0.0, 5.0, M_PI, M_PI / 2, 100);
|
||
} else if (path_type == "Straight Line") {
|
||
PathPoint start(0, 0, 0, 0);
|
||
PathPoint end(10, 0, 0, 0);
|
||
path.generateLine(start, end, 100);
|
||
} else if (path_type == "S-Curve") {
|
||
PathPoint p0(0, 0, 0, 0);
|
||
PathPoint p1(3, 2, 0, 0);
|
||
PathPoint p2(7, 2, 0, 0);
|
||
PathPoint p3(10, 0, 0, 0);
|
||
path.generateCubicBezier(p0, p1, p2, p3, 100);
|
||
}
|
||
|
||
// 验证路径
|
||
if (path.getPathPoints().empty()) {
|
||
QMessageBox::warning(this, "Error", "Invalid path!");
|
||
return;
|
||
}
|
||
|
||
// Set up tracker
|
||
tracker_->setReferencePath(path);
|
||
|
||
// 修复: 从路径起点获取初始状态，确保完美匹配
|
||
const auto& path_points = path.getPathPoints();
|
||
AGVModel::State initial_state;
|
||
if (!path_points.empty()) {
|
||
const PathPoint& start = path_points[0];
|
||
initial_state = AGVModel::State(start.x, start.y, start.theta);
|
||
} else {
|
||
initial_state = AGVModel::State(0.0, 0.0, 0.0);
|
||
}
|
||
tracker_->setInitialState(initial_state);
|
||
|
||
// Generate control sequence
|
||
QString algo = algorithm_combo_->currentText();
|
||
std::string algo_str = (algo == "Pure Pursuit") ? "pure_pursuit" : "stanley";
|
||
|
||
double dt = dt_spin_->value();
|
||
double horizon = horizon_spin_->value();
|
||
// 修复: 使用GUI中的速度参数
|
||
double desired_velocity = max_vel_spin_->value();
|
||
|
||
tracker_->generateControlSequence(algo_str, dt, horizon, desired_velocity);
|
||
const ControlSequence& sequence = tracker_->getControlSequence();
|
||
|
||
// Update visualization
|
||
visualization_->setPath(path);
|
||
visualization_->setControlSequence(sequence);
|
||
visualization_->setCurrentStep(0);
|
||
visualization_->setShowAnimation(true);
|
||
|
||
// Update table
|
||
updateTable(sequence);
|
||
|
||
// Update statistics
|
||
updateStatistics(sequence);
|
||
|
||
// Enable animation button
|
||
start_btn_->setEnabled(true);
|
||
start_btn_->setText("Start Animation");
|
||
animation_running_ = false;
|
||
}
|
||
|
||
void toggleAnimation() {
|
||
const ControlSequence& sequence = tracker_->getControlSequence();
|
||
if (sequence.size() == 0) return;
|
||
|
||
if (animation_running_) {
|
||
// Pause the animation
|
||
animation_timer_->stop();
|
||
start_btn_->setText("Resume Animation");
|
||
animation_running_ = false;
|
||
} else {
|
||
// Resume or start animation
|
||
// Only reset to beginning if animation has finished
|
||
if (animation_step_ >= sequence.size()) {
|
||
animation_step_ = 0;
|
||
}
|
||
animation_timer_->start(100); // 100ms interval
|
||
start_btn_->setText("Pause Animation");
|
||
animation_running_ = true;
|
||
}
|
||
}
|
||
|
||
void updateAnimation() {
|
||
const ControlSequence& sequence = tracker_->getControlSequence();
|
||
if (animation_step_ >= sequence.size()) {
|
||
animation_timer_->stop();
|
||
start_btn_->setText("Restart Animation");
|
||
animation_running_ = false;
|
||
return;
|
||
}
|
||
|
||
visualization_->setCurrentStep(animation_step_);
|
||
|
||
// Highlight current row in table
|
||
table_->selectRow(animation_step_);
|
||
|
||
animation_step_++;
|
||
}
|
||
|
||
private:
|
||
void updateAGVModel() {
|
||
double wheelbase = wheelbase_spin_->value();
|
||
double max_vel = max_vel_spin_->value();
|
||
double max_steer = max_steer_spin_->value() * M_PI / 180.0;
|
||
|
||
model_ = std::make_unique<AGVModel>(wheelbase, max_vel, max_steer);
|
||
tracker_ = std::make_unique<PathTracker>(*model_);
|
||
}
|
||
|
||
QDoubleSpinBox* createParamRow(const QString& label, double min, double max,
|
||
double value, QVBoxLayout* layout) {
|
||
QHBoxLayout* row = new QHBoxLayout();
|
||
row->addWidget(new QLabel(label, this));
|
||
|
||
QDoubleSpinBox* spin = new QDoubleSpinBox(this);
|
||
spin->setRange(min, max);
|
||
spin->setValue(value);
|
||
spin->setSingleStep((max - min) / 100.0);
|
||
spin->setDecimals(2);
|
||
row->addWidget(spin);
|
||
|
||
layout->addLayout(row);
|
||
return spin;
|
||
}
|
||
|
||
void updateTable(const ControlSequence& sequence) {
|
||
table_->setRowCount(sequence.size());
|
||
|
||
for (size_t i = 0; i < sequence.size(); ++i) {
|
||
table_->setItem(i, 0, new QTableWidgetItem(QString::number(i)));
|
||
table_->setItem(i, 1, new QTableWidgetItem(
|
||
QString::number(sequence.timestamps[i], 'f', 2)));
|
||
table_->setItem(i, 2, new QTableWidgetItem(
|
||
QString::number(sequence.controls[i].v, 'f', 4)));
|
||
table_->setItem(i, 3, new QTableWidgetItem(
|
||
QString::number(sequence.controls[i].delta * 180.0 / M_PI, 'f', 2)));
|
||
}
|
||
}
|
||
|
||
void updateStatistics(const ControlSequence& sequence) {
|
||
if (sequence.size() == 0) {
|
||
stats_label_->setText("No data");
|
||
return;
|
||
}
|
||
|
||
double avg_vel = 0, max_vel = -1e9, min_vel = 1e9;
|
||
double avg_steer = 0, max_steer = -1e9, min_steer = 1e9;
|
||
|
||
for (const auto& ctrl : sequence.controls) {
|
||
avg_vel += ctrl.v;
|
||
max_vel = std::max(max_vel, ctrl.v);
|
||
min_vel = std::min(min_vel, ctrl.v);
|
||
|
||
double delta_deg = ctrl.delta * 180.0 / M_PI;
|
||
avg_steer += delta_deg;
|
||
max_steer = std::max(max_steer, delta_deg);
|
||
min_steer = std::min(min_steer, delta_deg);
|
||
}
|
||
|
||
avg_vel /= sequence.size();
|
||
avg_steer /= sequence.size();
|
||
|
||
QString stats = QString(
|
||
"Total Steps: %1\n\n"
|
||
"Velocity:\n"
|
||
" Avg: %2 m/s\n"
|
||
" Max: %3 m/s\n"
|
||
" Min: %4 m/s\n\n"
|
||
"Steering:\n"
|
||
" Avg: %5°\n"
|
||
" Max: %6°\n"
|
||
" Min: %7°"
|
||
).arg(sequence.size())
|
||
.arg(avg_vel, 0, 'f', 4)
|
||
.arg(max_vel, 0, 'f', 4)
|
||
.arg(min_vel, 0, 'f', 4)
|
||
.arg(avg_steer, 0, 'f', 2)
|
||
.arg(max_steer, 0, 'f', 2)
|
||
.arg(min_steer, 0, 'f', 2);
|
||
|
||
stats_label_->setText(stats);
|
||
}
|
||
|
||
// Widgets
|
||
PathVisualizationWidget* visualization_;
|
||
QTableWidget* table_;
|
||
QComboBox* algorithm_combo_;
|
||
QComboBox* path_combo_;
|
||
QDoubleSpinBox* wheelbase_spin_;
|
||
QDoubleSpinBox* max_vel_spin_;
|
||
QDoubleSpinBox* max_steer_spin_;
|
||
QDoubleSpinBox* dt_spin_;
|
||
QDoubleSpinBox* horizon_spin_;
|
||
QPushButton* start_btn_;
|
||
QGroupBox* stats_group_;
|
||
QLabel* stats_label_;
|
||
|
||
// Model and tracker
|
||
std::unique_ptr<AGVModel> model_;
|
||
std::unique_ptr<PathTracker> tracker_;
|
||
|
||
// Animation
|
||
QTimer* animation_timer_;
|
||
int animation_step_;
|
||
bool animation_running_ = false;
|
||
|
||
//自定义路径
|
||
PathCurve custom_path_;
|
||
bool custom_path_loaded_ = false;
|
||
};
|
||
|
||
int main(int argc, char* argv[]) {
|
||
QApplication app(argc, argv);
|
||
|
||
MainWindow window;
|
||
window.show();
|
||
|
||
return app.exec();
|
||
}
|
||
|
||
#include "qt_gui_demo.moc"
|