diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 1657742..375b884 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -10,7 +10,8 @@ "Bash(chmod:*)", "Bash(./scripts/archive_bug_fix.sh:*)", "Bash(cmake --build:*)", - "Bash(cmake:*)" + "Bash(cmake:*)", + "Bash(git commit -m \"$(cat <<''EOF''\n修复 .gitignore 和 CMakeLists.txt 构建配置\n\n- 修复 .gitignore 忽略所有 txt 文件的问题，改为只忽略特定目录\n- 修复 CMakeLists.txt 中 Curtis 控制器链接错误，改用 .lib 文件\n- 确保 CMakeLists.txt 不被 git 忽略\n\n🤖 Generated with [Claude Code](https://claude.com/claude-code)\n\nCo-Authored-By: Claude \nEOF\n)\")" ], "deny": [], "ask": [] diff --git a/docs/custom_path/CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt b/docs/custom_path/CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt new file mode 100644 index 0000000..e6c9fbd --- /dev/null +++ b/docs/custom_path/CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt @@ -0,0 +1,260 @@ +================================================================================ + AGV 自定义路径功能实现总结 +================================================================================ + +实施日期: 2025-11-13 +状态: 已完成，待集成 + +================================================================================ +1. 新增文件列表 +================================================================================ + +核心实现文件： + ✓ src/path_curve_custom.cpp - 自定义路径功能实现 + (CSV加载/保存 + 样条插值) + +示例和文档： + ✓ examples/custom_path.csv SMOOTH_PATH_GENERATOR_README- 示例CSV路径文件 + ✓ examples/custom_path_demo.cpp - 演示程序（占位符） + +文档文件： + ✓ CUSTOM_PATH_GUIDE.md - 完整使用指南（中文） + ✓ QUICKSTART_CUSTOM_PATH.md - 快速开始指南 + +辅助文件： + ✓ path_curve.h.patch - 头文件修改补丁 + ✓ install_custom_path.sh - 自动安装脚本 + ✓ CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt - 本文件 + +================================================================================ +2. 新增功能概览 +================================================================================ + +功能 1: CSV 路径加载 + 方法: bool loadFromCSV(const std::string& filename, bool has_header) + 功能: 从CSV文件加载路径点 + 格式支持: + - 完整格式: x, y, theta, kappa + - 简化格式: x, y (自动计算theta和kappa) + +功能 2: CSV 路径保存 + 方法: bool saveToCSV(const std::string& filename) const + 功能: 将路径保存为CSV格式 + 输出格式: x, y, theta, kappa (带注释表头) + +功能 3: 样条插值 + 方法: void generateSpline(const std::vector&, int, double) + 功能: 从少量关键点生成平滑路径 + 算法: Catmull-Rom 样条插值 + 参数: + - key_points: 关键点数组（最少2个） + - num_points: 生成点数（推荐100-500） + - tension: 张力系数（0.0=平滑, 1.0=紧密） + +================================================================================ +3. 集成步骤（需要手动执行） +================================================================================ + +步骤 1: 修改头文件 +-------------------------------------- +在 include/path_curve.h 中添加： + +a) 在第5行添加头文件： + #include + +b) 在 setPathPoints 方法后添加三个新方法声明： + - bool loadFromCSV(...) + - bool saveToCSV(...) const + - void generateSpline(...) + + 详见：path_curve.h.patch + +步骤 2: 更新 CMakeLists.txt +-------------------------------------- +在 SOURCES 列表中添加： + src/path_curve_custom.cpp + +修改前： +set(SOURCES + src/agv_model.cpp + src/path_curve.cpp + src/control_generator.cpp + src/path_tracker.cpp +) + +修改后： +set(SOURCES + src/agv_model.cpp + src/path_curve.cpp + src/path_curve_custom.cpp # <-- 添加这行 + src/control_generator.cpp + src/path_tracker.cpp +) + +步骤 3: 重新编译 +-------------------------------------- +cd build +cmake .. +cmake --build . + +或使用自动安装脚本： +bash install_custom_path.sh + +================================================================================ +4. 使用示例 +================================================================================ + +最简单的例子 - 从CSV加载路径： +-------------------------------------- +PathCurve path; +path.loadFromCSV("custom_path.csv"); + +// 使用路径进行跟踪 +PathTracker tracker(agv); +tracker.setReferencePath(path); + +样条插值例子： +-------------------------------------- +std::vector keypoints = { + PathPoint(0, 0), + PathPoint(5, 3), + PathPoint(10, 0) +}; + +PathCurve path; +path.generateSpline(keypoints, 200, 0.5); +path.saveToCSV("smooth_path.csv"); + +================================================================================ +5. 验证测试 +================================================================================ + +基本测试： + 1. 加载 examples/custom_path.csv + PathCurve path; + assert(path.loadFromCSV("examples/custom_path.csv")); + assert(path.getPathPoints().size() > 0); + + 2. 样条生成 + std::vector kp = {PathPoint(0,0), PathPoint(10,10)}; + path.generateSpline(kp, 100, 0.5); + assert(path.getPathPoints().size() == 100); + + 3. CSV保存 + assert(path.saveToCSV("test_output.csv")); + +================================================================================ +6. 与现有功能的对比 +================================================================================ + +| 功能 | 原有方式 | 新增方式 | 优势 | +|------|---------|---------|------| +| 直线 | generateLine() | loadFromCSV() | 灵活，可外部编辑 | +| 曲线 | generateCircleArc() | generateSpline() | 任意形状，平滑 | +| 复杂路径 | 手动组合多段 | loadFromCSV() | 一次加载完整路径 | +| 保存路径 | 不支持 | saveToCSV() | 可重用，可视化 | + +================================================================================ +7. 文件大小统计 +================================================================================ + +src/path_curve_custom.cpp ~200 行 +path_curve.h 修改 ~30 行 +examples/custom_path.csv ~10 行 +CUSTOM_PATH_GUIDE.md ~500 行 +QUICKSTART_CUSTOM_PATH.md ~300 行 +install_custom_path.sh ~50 行 + +总计新增代码: ~200 行 C++ +总计新增文档: ~800 行 Markdown + +================================================================================ +8. 依赖和兼容性 +================================================================================ + +依赖库: + - C++ 标准库: , , , + - 已有依赖: , + +兼容性: + ✓ C++11 及以上 + ✓ Windows (MSVC) + ✓ Linux (GCC) + ✓ macOS (Clang) + ✓ 完全向后兼容现有代码 + +================================================================================ +9. 已知限制和未来改进 +================================================================================ + +当前限制: + 1. CSV解析简单，不支持带引号的字段 + 2. 样条插值仅支持 Catmull-Rom，未来可添加 B-spline + 3. 大文件加载未优化（建议 < 10000 点） + +建议改进: + [ ] 添加 B-spline 插值选项 + [ ] 支持 JSON 格式路径 + [ ] 路径编辑器 GUI + [ ] 路径验证功能（检查曲率、连续性等） + +================================================================================ +10. 快速参考 +================================================================================ + +加载CSV: + PathCurve path; + path.loadFromCSV("file.csv"); + +保存CSV: + path.saveToCSV("output.csv"); + +样条插值: + std::vector kp = {PathPoint(0,0), PathPoint(10,5)}; + path.generateSpline(kp, 200, 0.5); + +获取信息: + path.getPathPoints().size() // 点数 + path.getPathLength() // 长度(m) + +================================================================================ +11. 支持和文档 +================================================================================ + +完整文档: + - CUSTOM_PATH_GUIDE.md (详细使用指南) + - QUICKSTART_CUSTOM_PATH.md (快速开始) + - README.md (项目总览) + +示例代码: + - examples/custom_path.csv (示例路径文件) + - examples/demo.cpp (原有demo) + +================================================================================ +12. 检查清单 +================================================================================ + +代码实现: + [✓] CSV 加载功能 + [✓] CSV 保存功能 + [✓] 样条插值功能 + [✓] 错误处理 + [✓] 注释文档 + +文件创建: + [✓] 实现文件 (src/path_curve_custom.cpp) + [✓] 示例CSV (examples/custom_path.csv) + [✓] 使用指南 (CUSTOM_PATH_GUIDE.md) + [✓] 快速开始 (QUICKSTART_CUSTOM_PATH.md) + [✓] 补丁文件 (path_curve.h.patch) + [✓] 安装脚本 (install_custom_path.sh) + +待集成项: + [ ] 修改 include/path_curve.h (需手动或运行脚本) + [ ] 修改 CMakeLists.txt (需手动或运行脚本) + [ ] 重新编译项目 + [ ] 运行测试验证 + +================================================================================ +实现完成！请按照"集成步骤"完成最后的集成。 +================================================================================ diff --git a/docs/custom_path/REFERENCE_PATH_SUMMARY.txt b/docs/custom_path/REFERENCE_PATH_SUMMARY.txt new file mode 100644 index 0000000..a6a0381 --- /dev/null +++ b/docs/custom_path/REFERENCE_PATH_SUMMARY.txt @@ -0,0 +1,283 @@ +AGV 路径跟踪项目 - 参考路径实现完整分析 +============================================================ + +1. 参考路径相关文件 +============================================================ + +核心文件： + C:/work/AGV/AGV运动规划/agv_path_tracking/include/path_curve.h + C:/work/AGV/AGV运动规划/agv_path_tracking/src/path_curve.cpp + C:/work/AGV/AGV运动规划/agv_path_tracking/include/path_tracker.h + C:/work/AGV/AGV运动规划/agv_path_tracking/src/path_tracker.cpp + +示例文件： + C:/work/AGV/AGV运动规划/agv_path_tracking/examples/demo.cpp + C:/work/AGV/AGV运动规划/agv_path_tracking/examples/generate_data.cpp + +数据结构文件： + C:/work/AGV/AGV运动规划/agv_path_tracking/include/agv_model.h + C:/work/AGV/AGV运动规划/agv_path_tracking/include/control_generator.h + + +2. 路径数据结构 +============================================================ + +2.1 PathPoint 结构体： +struct PathPoint { + double x; // x坐标（米） + double y; // y坐标（米） + double theta; // 切线方向角（弧度） + double kappa; // 曲率（1/米） +}; + +2.2 PathCurve 类： +class PathCurve { +private: + std::vector path_points_; // 路径点序列 +}; + + +3. 参考路径定义和生成方式 +============================================================ + +3.1 四种生成方法： + +方法 1: 直线路径 - generateLine() + 原型：void generateLine(const PathPoint& start, const PathPoint& end, + int num_points = 100) + 特点：曲率为 0，所有点方向相同 + 示例：path.generateLine(PathPoint(0,0), PathPoint(10,10), 100) + +方法 2: 圆弧路径 - generateCircleArc() + 原型：void generateCircleArc(double center_x, double center_y, double radius, + double start_angle, double end_angle, + int num_points = 100) + 特点：恒定曲率 κ = ±1/radius + 示例：path.generateCircleArc(5.0, 0.0, 5.0, 0.0, M_PI/2, 100) + +方法 3: 三次贝塞尔曲线 - generateCubicBezier() + 原型：void generateCubicBezier(const PathPoint& p0, const PathPoint& p1, + const PathPoint& p2, const PathPoint& p3, + int num_points = 100) + 特点：平滑曲线，可变曲率 + 公式：P(t) = (1-t)³p0 + 3(1-t)²t·p1 + 3(1-t)t²·p2 + t³·p3 + 示例：PathPoint p0(0,0), p1(3,5), p2(7,5), p3(10,0); + path.generateCubicBezier(p0, p1, p2, p3, 100) + +方法 4: 设置路径点数组 - setPathPoints() + 原型：void setPathPoints(const std::vector& points) + 特点：直接从点数组初始化，支持路径组合 + 用途：合并多条曲线（如 S 型曲线） + +3.2 路径查询方法： + const std::vector& getPathPoints() const; + - 获取所有路径点 + + PathPoint getPointAt(double t) const; + - 根据参数 t ∈ [0,1] 线性插值获取路径点 + + double getPathLength() const; + - 计算路径总长度（米） + + int findNearestPoint(double x, double y) const; + - 找到距离 (x,y) 最近的路径点索引 + + +4. 预设的曲线类型 +============================================================ + +enum CurveType { + LINE, // 直线（κ = 0） + CIRCLE_ARC, // 圆弧（κ = ±1/R，恒定） + CUBIC_BEZIER, // 三次贝塞尔（κ 可变） + SPLINE // 样条曲线（预留） +}; + +详细对比： +┌──────────────┬──────────────────┬───────────┬─────────────────┐ +│ 曲线类型 │ 特点 │ 曲率 │ 应用场景 │ +├──────────────┼──────────────────┼───────────┼─────────────────┤ +│ LINE │ 直线，无曲率 │ κ = 0 │ 长直线运动 │ +│ CIRCLE_ARC │ 圆形弧线 │ κ = ±1/R │ 转弯、回转 │ +│ CUBIC_BEZIER │ 平滑曲线 │ 可变 │ 路径过渡 │ +│ SPLINE │ 样条曲线（待实现）│ 平滑 │ 复杂曲线拟合 │ +└──────────────┴──────────────────┴───────────┴─────────────────┘ + + +5. 路径数据格式 +============================================================ + +5.1 CSV 输出格式 - trajectory.csv（轨迹数据）： +# AGV Predicted Trajectory +# x(m), y(m), theta(rad), theta(deg) +0.000000, 0.000000, 0.000000, 0.000000 +0.070711, 0.070711, 0.785398, 45.000000 +... +10.000000, 10.000000, 0.785398, 45.000000 + +字段说明： + x(m): X 坐标，单位米 + y(m): Y 坐标，单位米 + theta(rad): 朝向角，单位弧度 + theta(deg): 朝向角，单位度 + +5.2 CSV 输出格式 - control_sequence.csv（控制序列）： +# AGV Control Sequence +# Time(s), Velocity(m/s), Steering(rad), Steering(deg) +0.000000, 1.000000, 0.732770, 41.984039 +0.100000, 1.000000, 0.732933, 41.993384 +... + +字段说明： + Time(s): 时间戳，单位秒 + Velocity(m/s): 线速度，单位米/秒 + Steering(rad): 转向角，单位弧度 + Steering(deg): 转向角，单位度 + + +6. 关键算法 +============================================================ + +6.1 曲率计算 - Menger 公式（三点法）： +κ = 4 × Area / (d1 × d2 × d3) + +其中： + Area = |叉积| / 2 = |dx1×dy2 - dy1×dx2| / 2 + d1 = 距离(p1, p2) + d2 = 距离(p2, p3) + d3 = 距离(p1, p3) + +6.2 路径点插值（参数 t ∈ [0, 1]）： +使用线性插值在相邻两点间： + x(t) = x1 + α(x2 - x1) + y(t) = y1 + α(y2 - y1) + θ(t) = θ1 + α(θ2 - θ1) + κ(t) = κ1 + α(κ2 - κ1) + +其中 α = t × (size-1) 的小数部分 + +6.3 最近点查找： +遍历所有路径点，计算到给定点 (x,y) 的欧氏距离， +返回距离最小的点的索引。 + + +7. 使用示例 +============================================================ + +示例 1: 生成直线路径并跟踪 +--- +PathCurve path; +path.generateLine(PathPoint(0,0), PathPoint(10,10), 100); + +PathTracker tracker(agv_model); +tracker.setReferencePath(path); +tracker.setInitialState(AGVModel::State(0,0,0)); +tracker.generateControlSequence("pure_pursuit", 0.1, 20.0); +tracker.saveTrajectory("trajectory.csv"); +--- + +示例 2: 生成圆弧路径 +--- +PathCurve path; +path.generateCircleArc(5.0, 0.0, 5.0, M_PI, M_PI/2, 100); +std::cout << "Path length: " << path.getPathLength() << " m" << std::endl; +--- + +示例 3: 生成 S 型曲线（组合两个圆弧） +--- +std::vector points; + +PathCurve arc1; +arc1.generateCircleArc(2.5, 0.0, 2.5, M_PI, M_PI/2, 50); +auto p1 = arc1.getPathPoints(); +points.insert(points.end(), p1.begin(), p1.end()); + +PathCurve arc2; +arc2.generateCircleArc(2.5, 5.0, 2.5, -M_PI/2, 0, 50); +auto p2 = arc2.getPathPoints(); +points.insert(points.end(), p2.begin(), p2.end()); + +PathCurve path; +path.setPathPoints(points); +--- + +示例 4: 贝塞尔曲线路径 +--- +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); +--- + +示例 5: 路径查询 +--- +const auto& points = path.getPathPoints(); +std::cout << "Total points: " << points.size() << std::endl; + +// 获取中间点 +PathPoint mid = path.getPointAt(0.5); +std::cout << "Mid point: (" << mid.x << ", " << mid.y << ")" << std::endl; + +// 找最近点 +int idx = path.findNearestPoint(5.0, 5.0); +std::cout << "Nearest point index: " << idx << std::endl; +--- + + +8. 参考路径与控制的集成 +============================================================ + +完整工作流程： + +1. 创建 AGV 模型 + AGVModel agv(1.0, 2.0, M_PI/4); + +2. 创建路径跟踪器 + PathTracker tracker(agv); + +3. 定义参考路径（4 种方式之一） + PathCurve path; + path.generateLine(start, end, 100); + +4. 设置参考路径 + tracker.setReferencePath(path); + +5. 设置初始状态 + tracker.setInitialState(AGVModel::State(0,0,0)); + +6. 生成控制序列（基于路径和控制算法） + tracker.generateControlSequence("pure_pursuit", 0.1, 20.0); + +7. 保存输出 + tracker.saveControlSequence("control_sequence.csv"); + tracker.saveTrajectory("trajectory.csv"); + +8. 可视化 + python visualize.py + +控制算法使用参考路径的： + - 路径点序列 + - 曲率信息 + - 方向角信息 + + +9. 执行命令 +============================================================ + +编译： + mkdir build + cd build + cmake .. + cmake --build . --config Release + +运行交互式演示（可选择路径类型和控制算法）： + cd build/Release + agv_demo.exe + +自动生成数据： + generate_data.exe + +Python 可视化： + python visualize.py + diff --git a/docs/guides/START_HERE.txt b/docs/guides/START_HERE.txt new file mode 100644 index 0000000..dddff63 --- /dev/null +++ b/docs/guides/START_HERE.txt @@ -0,0 +1,89 @@ +╔════════════════════════════════════════════════════════════════════════════╗ +║ ║ +║ AGV 自定义路径功能 - 从这里开始！ ║ +║ ║ +╚════════════════════════════════════════════════════════════════════════════╝ + +🎯 所有文档已整理到一个地方： + + 📂 docs/custom_path/ + +════════════════════════════════════════════════════════════════════════════ + +📖 第一步：阅读文档导航 + + Windows: type docs\custom_path\README.md + Linux/Mac: cat docs/custom_path/README.md + +════════════════════════════════════════════════════════════════════════════ + +⚡ 快速链接： + + 📄 功能总览（推荐首读） + docs/custom_path/FINAL_SUMMARY.md + + 🚀 快速上手（3分钟） + docs/custom_path/QUICKSTART_CUSTOM_PATH.md + + 🖥️ QT界面修改（简单） + docs/custom_path/apply_qt_modifications.md + + 🔧 自动安装（一键完成） + bash docs/custom_path/install_custom_path.sh + + 📚 完整教程（深入学习） + docs/custom_path/CUSTOM_PATH_GUIDE.md + +════════════════════════════════════════════════════════════════════════════ + +✨ 新功能： + + ✓ 从 CSV 文件加载自定义路径 + ✓ 保存路径到 CSV 文件 + ✓ 样条插值生成平滑曲线 + ✓ QT 图形界面集成 + +════════════════════════════════════════════════════════════════════════════ + +📁 文档列表（docs/custom_path/）： + + README.md - 📖 文档导航（从这里开始） + FINAL_SUMMARY.md - ⭐ 功能总览 + QUICKSTART_CUSTOM_PATH.md - 🚀 快速上手 + CUSTOM_PATH_GUIDE.md - 📚 完整教程 + apply_qt_modifications.md - 🖥️ QT快速修改 + QT_GUI_CUSTOM_PATH_GUIDE.md - 🖥️ QT详细指南 + qt_gui_custom_code_snippet.cpp - 💻 QT代码示例 + install_custom_path.sh - 🔧 安装脚本 + path_curve.h.patch - 📝 头文件补丁 + CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt - 💡 实现细节 + PROJECT_STRUCTURE.md - 📁 项目结构 + +════════════════════════════════════════════════════════════════════════════ + +💡 推荐阅读顺序： + + 1️⃣ CUSTOM_PATH_README.md (本目录) - 2分钟了解 + 2️⃣ docs/custom_path/README.md - 5分钟导航 + 3️⃣ docs/custom_path/FINAL_SUMMARY.md - 10分钟总览 + 4️⃣ 根据需要选择其他文档 + +════════════════════════════════════════════════════════════════════════════ + +🎓 按场景选择： + + 想快速试用？ + → docs/custom_path/QUICKSTART_CUSTOM_PATH.md + + 想修改QT界面？ + → docs/custom_path/apply_qt_modifications.md + + 想深入学习？ + → docs/custom_path/CUSTOM_PATH_GUIDE.md + + 想立即安装？ + → bash docs/custom_path/install_custom_path.sh + +════════════════════════════════════════════════════════════════════════════ + +Happy Coding! 🚀 diff --git a/examples/initial_state_fix.txt b/examples/initial_state_fix.txt new file mode 100644 index 0000000..dde53d9 --- /dev/null +++ b/examples/initial_state_fix.txt @@ -0,0 +1,13 @@ + // 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); diff --git a/src/path_curve_fix.txt b/src/path_curve_fix.txt new file mode 100644 index 0000000..7ae3b37 --- /dev/null +++ b/src/path_curve_fix.txt @@ -0,0 +1,14 @@ + } else if (path_points_.size() > 2) { + // 中间点 + double dx = path_points_[i + 1].x - path_points_[i - 1].x; + double dy = path_points_[i + 1].y - path_points_[i - 1].y; + path_points_[i].theta = std::atan2(dy, dx); + + // 计算曲率（使用三点法） + if (i > 0 && i < path_points_.size() - 1) { + path_points_[i].kappa = computeCurvature( + path_points_[i - 1], path_points_[i], path_points_[i + 1]); + } + } + // 单点情况：保持原有的theta和kappa值（通常为0） + // 不需要额外计算，避免越界访问