213 lines
7.1 KiB
C++
213 lines
7.1 KiB
C++
// ============================================================================
|
||
// QT GUI 自定义路径功能 - 代码片段
|
||
// 将这些代码添加到 qt_gui_demo.cpp 中对应位置
|
||
// ============================================================================
|
||
|
||
// ----------------------------------------------------------------------------
|
||
// 1. 头文件部分 (第1-16行附近)
|
||
// ----------------------------------------------------------------------------
|
||
#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 <QFileDialog> // 新增
|
||
#include <QMessageBox> // 新增
|
||
#include <QInputDialog> // 新增
|
||
#include <cmath>
|
||
|
||
// ----------------------------------------------------------------------------
|
||
// 2. 路径类型选择 (MainWindow构造函数中，约第275-280行)
|
||
// ----------------------------------------------------------------------------
|
||
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_);
|
||
|
||
// ----------------------------------------------------------------------------
|
||
// 3. MainWindow 类成员变量 (private部分，约第527-530行)
|
||
// ----------------------------------------------------------------------------
|
||
private:
|
||
// ... 其他成员变量 ...
|
||
QTimer* animation_timer_;
|
||
int animation_step_;
|
||
bool animation_running_ = false;
|
||
|
||
// 新增: 自定义路径支持
|
||
PathCurve custom_path_;
|
||
bool custom_path_loaded_ = false;
|
||
};
|
||
|
||
// ----------------------------------------------------------------------------
|
||
// 4. generateControl() 方法 - 完整替换版本
|
||
// ----------------------------------------------------------------------------
|
||
void generateControl() {
|
||
updateAGVModel();
|
||
|
||
PathCurve path;
|
||
QString path_type = path_combo_->currentText();
|
||
|
||
// === 新增: CSV文件加载 ===
|
||
if (path_type == "Load from CSV") {
|
||
QString filename = QFileDialog::getOpenFileName(
|
||
this,
|
||
"Open CSV Path File",
|
||
"",
|
||
"CSV Files (*.csv);;All Files (*)");
|
||
|
||
if (filename.isEmpty()) {
|
||
return; // User cancelled
|
||
}
|
||
|
||
if (!path.loadFromCSV(filename.toStdString(), true)) {
|
||
QMessageBox::warning(
|
||
this,
|
||
"Load Error",
|
||
"Failed to load CSV file!\n\n"
|
||
"Please check:\n"
|
||
"- File format (x,y per line)\n"
|
||
"- File exists and readable");
|
||
return;
|
||
}
|
||
|
||
QMessageBox::information(
|
||
this,
|
||
"Load Success",
|
||
QString("Loaded %1 points from CSV\nPath length: %2 meters")
|
||
.arg(path.getPathPoints().size())
|
||
.arg(path.getPathLength(), 0, 'f', 2));
|
||
}
|
||
// === 新增: 样条插值 ===
|
||
else if (path_type == "Custom Spline") {
|
||
bool ok;
|
||
int num_points = QInputDialog::getInt(
|
||
this,
|
||
"Spline Key Points",
|
||
"Enter 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 Input",
|
||
QString("Point %1 - X coordinate:").arg(i + 1),
|
||
i * 3.0, -100.0, 100.0, 2, &ok);
|
||
if (!ok) return;
|
||
|
||
double y = QInputDialog::getDouble(
|
||
this,
|
||
"Key Point Input",
|
||
QString("Point %1 - Y coordinate:").arg(i + 1),
|
||
(i % 2 == 0) ? 0.0 : 3.0, -100.0, 100.0, 2, &ok);
|
||
if (!ok) return;
|
||
|
||
key_points.push_back(PathPoint(x, y));
|
||
}
|
||
|
||
int total_points = QInputDialog::getInt(
|
||
this,
|
||
"Spline Parameters",
|
||
"Total points to generate:",
|
||
200, 50, 1000, 50, &ok);
|
||
if (!ok) total_points = 200;
|
||
|
||
double tension = QInputDialog::getDouble(
|
||
this,
|
||
"Spline Parameters",
|
||
"Tension (0.0=smooth, 1.0=tight):",
|
||
0.5, 0.0, 1.0, 1, &ok);
|
||
if (!ok) tension = 0.5;
|
||
|
||
path.generateSpline(key_points, total_points, tension);
|
||
|
||
QMessageBox::information(
|
||
this,
|
||
"Spline Generated",
|
||
QString("Generated spline path:\n"
|
||
"Key points: %1\n"
|
||
"Total points: %2\n"
|
||
"Path length: %3 m")
|
||
.arg(key_points.size())
|
||
.arg(path.getPathPoints().size())
|
||
.arg(path.getPathLength(), 0, 'f', 2));
|
||
}
|
||
// === 原有路径类型 ===
|
||
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,
|
||
"Invalid Path",
|
||
"Path has no points!");
|
||
return;
|
||
}
|
||
|
||
// === 以下代码保持不变 ===
|
||
tracker_->setReferencePath(path);
|
||
AGVModel::State initial_state(0.0, 0.0, 0.0);
|
||
tracker_->setInitialState(initial_state);
|
||
|
||
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();
|
||
|
||
tracker_->generateControlSequence(algo_str, dt, horizon);
|
||
const ControlSequence& sequence = tracker_->getControlSequence();
|
||
|
||
visualization_->setPath(path);
|
||
visualization_->setControlSequence(sequence);
|
||
visualization_->setCurrentStep(0);
|
||
visualization_->setShowAnimation(true);
|
||
|
||
updateTable(sequence);
|
||
updateStatistics(sequence);
|
||
|
||
start_btn_->setEnabled(true);
|
||
start_btn_->setText("Start Animation");
|
||
animation_running_ = false;
|
||
}
|
||
|
||
// ============================================================================
|
||
// 使用说明:
|
||
//
|
||
// 1. 将上述代码片段复制到 qt_gui_demo.cpp 的对应位置
|
||
// 2. 重新编译: cd build && cmake .. && cmake --build .
|
||
// 3. 运行: ./agv_qt_gui
|
||
// 4. 在界面中选择 "Load from CSV" 或 "Custom Spline"
|
||
// 5. 点击 "Generate Control" 按钮
|
||
// 6. 按照提示操作
|
||
// ============================================================================
|