initial

examples/demo.cpp

@@ -0,0 +1,153 @@
+#include "path_tracker.h"
+#include <iostream>
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+int main() {
+    std::cout << "========================================" << std::endl;
+    std::cout << "    Single Steering Wheel AGV Path Tracking Control System Demo" << std::endl;
+    std::cout << "========================================\n" << std::endl;
+
+    // 1. Create AGV model
+    double wheelbase = 1.0;           // Wheelbase 1.0m
+    double max_velocity = 2.0;        // Max velocity 2.0 m/s
+    double max_steering = M_PI / 4;   // Max steering angle 45 degrees
+    AGVModel agv_model(wheelbase, max_velocity, max_steering);
+
+    std::cout << "AGV Parameters:" << std::endl;
+    std::cout << "  Wheelbase: " << wheelbase << " m" << std::endl;
+    std::cout << "  Max Velocity: " << max_velocity << " m/s" << std::endl;
+    std::cout << "  Max Steering Angle: " << (max_steering * 180.0 / M_PI) << " degrees" << std::endl;
+
+    // 2. Create path tracker
+    PathTracker tracker(agv_model);
+
+    // 3. Define reference path
+    PathCurve path;
+
+    std::cout << "Please select path type:" << std::endl;
+    std::cout << "1. Straight line path" << std::endl;
+    std::cout << "2. Circular arc path" << std::endl;
+    std::cout << "3. Bezier curve path" << std::endl;
+    std::cout << "4. S-curve path (combined)" << std::endl;
+    std::cout << "Enter choice (1-4): ";
+
+    int choice;
+    std::cin >> choice;
+
+    switch (choice) {
+        case 1: {
+            // Straight line: from (0,0) to (10,10)
+            PathPoint start(0.0, 0.0);
+            PathPoint end(10.0, 10.0);
+            path.generateLine(start, end, 100);
+            std::cout << "\nGenerated straight line path: (0,0) -> (10,10)" << std::endl;
+            break;
+        }
+        case 2: {
+            // Circular arc: center (5,0), radius 5m, from 0 to 90 degrees
+            path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI / 2, 100);
+            std::cout << "\nGenerated circular arc path: center (5,0), radius 5m" << std::endl;
+            break;
+        }
+        case 3: {
+            // Bezier curve
+            PathPoint p0(0.0, 0.0);
+            PathPoint p1(3.0, 5.0);
+            PathPoint p2(7.0, 5.0);
+            PathPoint p3(10.0, 0.0);
+            path.generateCubicBezier(p0, p1, p2, p3, 100);
+            std::cout << "\nGenerated Bezier curve path" << std::endl;
+            break;
+        }
+        case 4: {
+            // S-curve (two connected arcs)
+            std::vector<PathPoint> points;
+
+            // First segment: arc to the right
+            PathCurve arc1;
+            arc1.generateCircleArc(2.5, 0.0, 2.5, M_PI, M_PI / 2, 50);
+            auto arc1_points = arc1.getPathPoints();
+            points.insert(points.end(), arc1_points.begin(), arc1_points.end());
+
+            // Second segment: arc to the left
+            PathCurve arc2;
+            arc2.generateCircleArc(2.5, 5.0, 2.5, -M_PI / 2, 0, 50);
+            auto arc2_points = arc2.getPathPoints();
+            points.insert(points.end(), arc2_points.begin(), arc2_points.end());
+
+            path.setPathPoints(points);
+            std::cout << "\nGenerated S-curve path" << std::endl;
+            break;
+        }
+        default:
+            std::cout << "Invalid choice, using default straight line path" << std::endl;
+            PathPoint start(0.0, 0.0);
+            PathPoint end(10.0, 10.0);
+            path.generateLine(start, end, 100);
+            break;
+    }
+
+    tracker.setReferencePath(path);
+    std::cout << "Path length: " << path.getPathLength() << " m" << std::endl;
+    std::cout << "Path points: " << path.getPathPoints().size() << std::endl;
+
+    // 4. Set initial state
+    AGVModel::State initial_state(0.0, 0.0, 0.0);  // Start at (0,0), heading 0 degrees
+    tracker.setInitialState(initial_state);
+    std::cout << "\nInitial state: x=" << initial_state.x
+              << ", y=" << initial_state.y
+              << ", theta=" << (initial_state.theta * 180.0 / M_PI) << " degrees" << std::endl;
+
+    // 5. Select control algorithm
+    std::cout << "\nPlease select control algorithm:" << std::endl;
+    std::cout << "1. Pure Pursuit" << std::endl;
+    std::cout << "2. Stanley" << std::endl;
+    std::cout << "Enter choice (1-2): ";
+
+    int algo_choice;
+    std::cin >> algo_choice;
+
+    std::string algorithm = (algo_choice == 2) ? "stanley" : "pure_pursuit";
+    std::cout << "\nUsing algorithm: " << algorithm << std::endl;
+
+    // 6. Generate control sequence
+    std::cout << "\nGenerating control sequence..." << std::endl;
+    double dt = 0.1;        // Time step 0.1s
+    double horizon = 20.0;  // Prediction horizon 20s
+
+    if (!tracker.generateControlSequence(algorithm, dt, horizon)) {
+        std::cerr << "Control sequence generation failed!" << std::endl;
+        return 1;
+    }
+
+    std::cout << "Control sequence generation completed!" << std::endl;
+
+    // 7. Display control sequence
+    tracker.printControlSequence();
+
+    // 8. Save to file
+    std::cout << "\nSave control sequence to file? (y/n): ";
+    char save_choice;
+    std::cin >> save_choice;
+
+    if (save_choice == 'y' || save_choice == 'Y') {
+        tracker.saveControlSequence("control_sequence.csv");
+        tracker.saveTrajectory("trajectory.csv");
+        std::cout << "\nFiles saved!" << std::endl;
+        std::cout << "  - control_sequence.csv (control sequence)" << std::endl;
+        std::cout << "  - trajectory.csv (predicted trajectory)" << std::endl;
+        std::cout << "\nNote: You can visualize CSV files using Python/MATLAB/Excel" << std::endl;
+    }
+
+    std::cout << "\n========================================" << std::endl;
+    std::cout << "           Demo Program Ended" << std::endl;
+    std::cout << "========================================" << std::endl;
+
+    return 0;
+}

examples/fix_initial_state.py

@@ -0,0 +1,41 @@
+import sys
+
+# Read the file
+with open('qt_gui_demo.cpp', 'r', encoding='utf-8') as f:
+    lines = f.readlines()
+
+# Find and replace the initial state section (around line 448-451)
+new_lines = []
+skip_count = 0
+for i, line in enumerate(lines):
+    if skip_count > 0:
+        skip_count -= 1
+        continue
+
+    if i >= 447 and 'Set up tracker' in line:
+        # Add the original comment
+        new_lines.append(line)
+        # Add the next line (setReferencePath)
+        new_lines.append(lines[i+1])
+        # Add blank line
+        new_lines.append('\n')
+        # Add the new initial state code
+        new_lines.append('        // 修复: 从路径起点获取初始状态，确保完美匹配\n')
+        new_lines.append('        const auto& path_points = path.getPathPoints();\n')
+        new_lines.append('        AGVModel::State initial_state;\n')
+        new_lines.append('        if (!path_points.empty()) {\n')
+        new_lines.append('            const PathPoint& start = path_points[0];\n')
+        new_lines.append('            initial_state = AGVModel::State(start.x, start.y, start.theta);\n')
+        new_lines.append('        } else {\n')
+        new_lines.append('            initial_state = AGVModel::State(0.0, 0.0, 0.0);\n')
+        new_lines.append('        }\n')
+        new_lines.append('        tracker_->setInitialState(initial_state);\n')
+        skip_count = 3  # Skip the next 3 lines (setReferencePath, old initial_state, setInitialState)
+    else:
+        new_lines.append(line)
+
+# Write back
+with open('qt_gui_demo.cpp', 'w', encoding='utf-8') as f:
+    f.writelines(new_lines)
+
+print("Initial state fix applied successfully!")

examples/generate_data.cpp

@@ -0,0 +1,36 @@
+#include "path_tracker.h"
+#include <iostream>
+
+int main() {
+    std::cout << "Generating AGV demo data files..." << std::endl;
+
+    // Create AGV model
+    AGVModel agv_model(1.0, 2.0, M_PI / 4);
+
+    // Create path tracker
+    PathTracker tracker(agv_model);
+
+    // Create circular arc path from (0,0) to (5,5)
+    PathCurve path;
+    path.generateCircleArc(5.0, 0.0, 5.0, M_PI, 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 using Pure Pursuit
+    std::cout << "Generating control sequence with Pure Pursuit..." << std::endl;
+    tracker.generateControlSequence("pure_pursuit", 0.1, 10.0);
+
+    // Save files
+    tracker.saveControlSequence("control_sequence.csv");
+    tracker.saveTrajectory("trajectory.csv");
+
+    std::cout << "\nData files generated successfully!" << std::endl;
+    std::cout << "  - control_sequence.csv" << std::endl;
+    std::cout << "  - trajectory.csv" << std::endl;
+    std::cout << "\nYou can now run: python visualize.py" << std::endl;
+
+    return 0;
+}

examples/generate_smooth_path.cpp

@@ -0,0 +1,261 @@
+#include "path_curve.h"
+#include <iostream>
+#include <string>
+
+/**
+ * @brief 平滑路径生成器类
+ * 用于生成各种类型的平滑路径并保存为CSV文件
+ */
+class SmoothPathGenerator {
+public:
+    /**
+     * @brief 生成圆弧平滑路径
+     * @param filename 输出文件名
+     * @param center_x 圆心X坐标
+     * @param center_y 圆心Y坐标
+     * @param radius 半径
+     * @param start_angle 起始角度（弧度）
+     * @param end_angle 终止角度（弧度）
+     * @param num_points 路径点数量
+     */
+    static bool generateCircleArc(const std::string& filename,
+                                  double center_x, double center_y, double radius,
+                                  double start_angle, double end_angle,
+                                  int num_points = 200) {
+        PathCurve path;
+        path.generateCircleArc(center_x, center_y, radius,
+                              start_angle, end_angle, num_points);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ Circle arc path saved: " << filename << std::endl;
+            std::cout << "  Points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 生成S型曲线（三次贝塞尔曲线）
+     * @param filename 输出文件名
+     * @param start_x 起点X
+     * @param start_y 起点Y
+     * @param end_x 终点X
+     * @param end_y 终点Y
+     * @param control_offset 控制点偏移量
+     * @param num_points 路径点数量
+     */
+    static bool generateSCurve(const std::string& filename,
+                               double start_x, double start_y,
+                               double end_x, double end_y,
+                               double control_offset = 3.0,
+                               int num_points = 200) {
+        PathPoint p0(start_x, start_y);
+        PathPoint p1(start_x + control_offset, start_y + control_offset);
+        PathPoint p2(end_x - control_offset, end_y + control_offset);
+        PathPoint p3(end_x, end_y);
+
+        PathCurve path;
+        path.generateCubicBezier(p0, p1, p2, p3, num_points);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ S-curve path saved: " << filename << std::endl;
+            std::cout << "  Points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 生成样条曲线路径
+     * @param filename 输出文件名
+     * @param key_points 关键点数组
+     * @param num_points 生成的路径点总数
+     * @param tension 张力参数（0-1，越大越紧）
+     */
+    static bool generateSpline(const std::string& filename,
+                               const std::vector<PathPoint>& key_points,
+                               int num_points = 200,
+                               double tension = 0.5) {
+        if (key_points.size() < 2) {
+            std::cerr << "✗ Error: At least 2 key points required" << std::endl;
+            return false;
+        }
+
+        PathCurve path;
+        path.generateSpline(key_points, num_points, tension);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ Spline path saved: " << filename << std::endl;
+            std::cout << "  Key points: " << key_points.size() << std::endl;
+            std::cout << "  Total points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 生成复杂的平滑路径（组合多个关键点）
+     * @param filename 输出文件名
+     * @param num_points 路径点数量
+     */
+    static bool generateComplexPath(const std::string& filename,
+                                    int num_points = 300) {
+        // 定义关键点：模拟AGV在仓库中的复杂路径
+        std::vector<PathPoint> key_points = {
+            PathPoint(0.0, 0.0),      // 起点
+            PathPoint(5.0, 0.5),      // 第一段轻微转弯
+            PathPoint(8.0, 3.0),      // 上升
+            PathPoint(10.0, 6.0),     // 继续上升
+            PathPoint(11.0, 9.0),     // 接近顶部
+            PathPoint(10.5, 12.0),    // 转向
+            PathPoint(8.0, 13.0),     // 左转
+            PathPoint(5.0, 13.5),     // 继续左转
+            PathPoint(2.0, 12.0),     // 下降
+            PathPoint(0.0, 10.0)      // 终点
+        };
+
+        PathCurve path;
+        path.generateSpline(key_points, num_points, 0.3);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ Complex smooth path saved: " << filename << std::endl;
+            std::cout << "  Key points: " << key_points.size() << std::endl;
+            std::cout << "  Total points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 生成环形平滑路径
+     * @param filename 输出文件名
+     * @param radius 半径
+     * @param num_points 路径点数量
+     */
+    static bool generateLoop(const std::string& filename,
+                            double radius = 5.0,
+                            int num_points = 300) {
+        PathCurve path;
+        path.generateCircleArc(0.0, 0.0, radius, 0.0, 2 * M_PI, num_points);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ Loop path saved: " << filename << std::endl;
+            std::cout << "  Radius: " << radius << " m" << std::endl;
+            std::cout << "  Points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 生成Figure-8路径（8字形）
+     * @param filename 输出文件名
+     * @param size 8字大小
+     * @param num_points 路径点数量
+     */
+    static bool generateFigure8(const std::string& filename,
+                                double size = 5.0,
+                                int num_points = 400) {
+        std::vector<PathPoint> key_points = {
+            PathPoint(0.0, 0.0),
+            PathPoint(size, size),
+            PathPoint(size * 2, 0.0),
+            PathPoint(size, -size),
+            PathPoint(0.0, 0.0)
+        };
+
+        PathCurve path;
+        path.generateSpline(key_points, num_points, 0.4);
+
+        if (path.saveToCSV(filename)) {
+            std::cout << "✓ Figure-8 path saved: " << filename << std::endl;
+            std::cout << "  Size: " << size << " m" << std::endl;
+            std::cout << "  Points: " << num_points << std::endl;
+            std::cout << "  Length: " << path.getPathLength() << " m" << std::endl;
+            return true;
+        }
+        return false;
+    }
+
+    /**
+     * @brief 打印使用说明
+     */
+    static void printUsage() {
+        std::cout << "\n========================================" << std::endl;
+        std::cout << "   Smooth Path Generator" << std::endl;
+        std::cout << "========================================\n" << std::endl;
+        std::cout << "This tool generates various smooth paths for AGV navigation.\n" << std::endl;
+        std::cout << "Available path types:" << std::endl;
+        std::cout << "  1. Simple smooth path (default)" << std::endl;
+        std::cout << "  2. Circle arc" << std::endl;
+        std::cout << "  3. S-curve" << std::endl;
+        std::cout << "  4. Complex path" << std::endl;
+        std::cout << "  5. Loop path" << std::endl;
+        std::cout << "  6. Figure-8 path" << std::endl;
+        std::cout << "\nAll paths will be saved as CSV files.\n" << std::endl;
+    }
+};
+
+int main(int argc, char* argv[]) {
+    SmoothPathGenerator::printUsage();
+
+    std::cout << "Generating smooth paths...\n" << std::endl;
+
+    // 1. 生成默认的平滑路径 - smooth_path.csv
+    std::cout << "[1] Generating default smooth path..." << std::endl;
+    std::vector<PathPoint> default_key_points = {
+        PathPoint(0.0, 0.0),
+        PathPoint(3.0, 1.0),
+        PathPoint(6.0, 3.0),
+        PathPoint(9.0, 3.5),
+        PathPoint(12.0, 3.0)
+    };
+    SmoothPathGenerator::generateSpline("smooth_path.csv", default_key_points, 200, 0.5);
+
+    // 2. 生成圆弧路径
+    std::cout << "\n[2] Generating circle arc path..." << std::endl;
+    SmoothPathGenerator::generateCircleArc("smooth_path_arc.csv",
+                                           5.0, 0.0, 5.0,
+                                           M_PI, M_PI / 2, 150);
+
+    // 3. 生成S型曲线
+    std::cout << "\n[3] Generating S-curve path..." << std::endl;
+    SmoothPathGenerator::generateSCurve("smooth_path_scurve.csv",
+                                        0.0, 0.0, 10.0, 0.0, 2.5, 200);
+
+    // 4. 生成复杂路径
+    std::cout << "\n[4] Generating complex path..." << std::endl;
+    SmoothPathGenerator::generateComplexPath("smooth_path_complex.csv", 300);
+
+    // 5. 生成环形路径
+    std::cout << "\n[5] Generating loop path..." << std::endl;
+    SmoothPathGenerator::generateLoop("smooth_path_loop.csv", 5.0, 300);
+
+    // 6. 生成8字形路径
+    std::cout << "\n[6] Generating Figure-8 path..." << std::endl;
+    SmoothPathGenerator::generateFigure8("smooth_path_figure8.csv", 4.0, 400);
+
+    std::cout << "\n========================================" << std::endl;
+    std::cout << "✓ All smooth paths generated successfully!" << std::endl;
+    std::cout << "========================================\n" << std::endl;
+
+    std::cout << "Generated files:" << std::endl;
+    std::cout << "  • smooth_path.csv        - Default smooth path" << std::endl;
+    std::cout << "  • smooth_path_arc.csv    - Circle arc" << std::endl;
+    std::cout << "  • smooth_path_scurve.csv - S-curve" << std::endl;
+    std::cout << "  • smooth_path_complex.csv - Complex path" << std::endl;
+    std::cout << "  • smooth_path_loop.csv   - Loop path" << std::endl;
+    std::cout << "  • smooth_path_figure8.csv - Figure-8 path" << std::endl;
+
+    std::cout << "\nYou can load these CSV files in the Qt GUI application:" << std::endl;
+    std::cout << "  1. Run: ./build/Debug/agv_qt_gui.exe" << std::endl;
+    std::cout << "  2. Select 'Load from CSV' in Path Type" << std::endl;
+    std::cout << "  3. Choose one of the generated CSV files" << std::endl;
+
+    return 0;
+}

examples/gui_demo.cpp

@@ -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;
+}

examples/qt_gui_demo.cpp

@@ -0,0 +1,651 @@
+#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"

examples/qt_gui_demo.cpp.backup

@@ -0,0 +1,638 @@
+#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, 30.0, 10.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()) {
+                if (custom_path_.loadFromCSV(filename.toStdString(), 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)");
+            if (!filename.isEmpty() && custom_path_loaded_) {
+                if (custom_path_.saveToCSV(filename.toStdString())) {
+                    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);
+        AGVModel::State initial_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();
+
+        tracker_->generateControlSequence(algo_str, dt, horizon);
+        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"

examples/qt_gui_demo.cpp.backup3

@@ -0,0 +1,640 @@
+#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);
+        AGVModel::State initial_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();
+
+        tracker_->generateControlSequence(algo_str, dt, horizon);
+        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"

examples/qt_gui_enhanced.cpp

@@ -0,0 +1,541 @@
+#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>
+
+/**
+ * @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;
+        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_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, 30.0, 10.0, control_layout);
+
+        right_layout->addWidget(control_group);
+
+        // 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 == "Circle Arc") {
+            // Circle arc from (0,0) to (5,5): center at (5,0), radius 5, from 180° to 90°
+            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);
+        }
+
+        // Set up tracker
+        tracker_->setReferencePath(path);
+        AGVModel::State initial_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();
+
+        tracker_->generateControlSequence(algo_str, dt, horizon);
+        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;
+};
+
+int main(int argc, char* argv[]) {
+    QApplication app(argc, argv);
+
+    MainWindow window;
+    window.show();
+
+    return app.exec();
+}
+
+#include "qt_gui_demo.moc"