commit for push
This commit is contained in:
CaiXiang
@@ -10,7 +10,8 @@
|
|||||||
"Bash(chmod:*)",
|
"Bash(chmod:*)",
|
||||||
"Bash(./scripts/archive_bug_fix.sh:*)",
|
"Bash(./scripts/archive_bug_fix.sh:*)",
|
||||||
"Bash(cmake --build:*)",
|
"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 <noreply@anthropic.com>\nEOF\n)\")"
|
||||||
],
|
],
|
||||||
"deny": [],
|
"deny": [],
|
||||||
"ask": []
|
"ask": []
|
||||||
|
|||||||
260
docs/custom_path/CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt
Normal file
260
docs/custom_path/CUSTOM_PATH_IMPLEMENTATION_SUMMARY.txt
Normal file
@@ -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<PathPoint>&, 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 <string>
|
||||||
|
|
||||||
|
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<PathPoint> 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<PathPoint> 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++ 标准库: <fstream>, <sstream>, <string>, <iostream>
|
||||||
|
- 已有依赖: <vector>, <cmath>
|
||||||
|
|
||||||
|
兼容性:
|
||||||
|
✓ 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<PathPoint> 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 (需手动或运行脚本)
|
||||||
|
[ ] 重新编译项目
|
||||||
|
[ ] 运行测试验证
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
实现完成！请按照"集成步骤"完成最后的集成。
|
||||||
|
================================================================================
|
||||||
283
docs/custom_path/REFERENCE_PATH_SUMMARY.txt
Normal file
283
docs/custom_path/REFERENCE_PATH_SUMMARY.txt
Normal file
@@ -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<PathPoint> 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<PathPoint>& points)
|
||||||
|
特点：直接从点数组初始化，支持路径组合
|
||||||
|
用途：合并多条曲线（如 S 型曲线）
|
||||||
|
|
||||||
|
3.2 路径查询方法：
|
||||||
|
const std::vector<PathPoint>& 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<PathPoint> 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
|
||||||
|
|
||||||
89
docs/guides/START_HERE.txt
Normal file
89
docs/guides/START_HERE.txt
Normal file
@@ -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! 🚀
|
||||||
13
examples/initial_state_fix.txt
Normal file
13
examples/initial_state_fix.txt
Normal file
@@ -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);
|
||||||
14
src/path_curve_fix.txt
Normal file
14
src/path_curve_fix.txt
Normal file
@@ -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）
|
||||||
|
// 不需要额外计算，避免越界访问
|
||||||
Reference in New Issue
Block a user