initial

2025-11-14 16:09:58 +08:00
commit af65c2425d
74 changed files with 14650 additions and 0 deletions
--- a/docs/custom_path/CUSTOM_PATH_GUIDE.md
+++ b/docs/custom_path/CUSTOM_PATH_GUIDE.md
@@ -0,0 +1,327 @@
+# AGV 自定义路径功能使用指南
+
+## 概述
+
+本指南介绍如何使用新增的自定义路径功能，包括：
+1. **从CSV文件加载路径**
+2. **保存路径到CSV文件**
+3. **样条插值生成平滑曲线**
+
+## 功能特性
+
+### 1. CSV 文件加载/保存
+
+支持两种CSV格式：
+- **完整格式**：`x, y, theta, kappa`
+- **简化格式**：`x, y` （theta和kappa会自动计算）
+
+### 2. 样条插值
+
+使用 Catmull-Rom 样条插值，只需提供少量关键点，自动生成平滑的路径曲线。
+
+## 安装步骤
+
+### 第一步：修改头文件
+
+在 `include/path_curve.h` 中添加以下方法声明（在 `setPathPoints` 方法之后）：
+
+```cpp
+// 在第77行之后添加：
+
+/**
+ * @brief 从CSV文件加载路径点
+ * @param filename CSV文件路径
+ * @param has_header 是否包含表头（默认true）
+ * @return 是否加载成功
+ *
+ * CSV格式支持以下两种：
+ * 1. 完整格式：x, y, theta, kappa
+ * 2. 简化格式：x, y （theta和kappa将自动计算）
+ */
+bool loadFromCSV(const std::string& filename, bool has_header = true);
+
+/**
+ * @brief 将路径点保存到CSV文件
+ * @param filename CSV文件路径
+ * @return 是否保存成功
+ */
+bool saveToCSV(const std::string& filename) const;
+
+/**
+ * @brief 使用样条插值生成路径
+ * @param key_points 关键路径点（只需指定x和y）
+ * @param num_points 生成的路径点总数
+ * @param tension 张力参数（0.0-1.0，控制曲线平滑度，默认0.5）
+ *
+ * 使用 Catmull-Rom 样条插值，生成经过所有关键点的平滑曲线
+ */
+void generateSpline(const std::vector<PathPoint>& key_points,
+                   int num_points = 100,
+                   double tension = 0.5);
+```
+
+同时在文件开头添加 string 头文件：
+```cpp
+#include <string>
+```
+
+### 第二步：添加实现文件到编译
+
+在 `src/CMakeLists.txt` 或主 `CMakeLists.txt` 中添加：
+```cmake
+add_library(path_curve_lib
+    src/path_curve.cpp
+    src/path_curve_custom.cpp  # 新增
+    # ... 其他文件
+)
+```
+
+### 第三步：重新编译
+
+```bash
+cd build
+cmake ..
+cmake --build .
+```
+
+## 使用示例
+
+### 示例 1：从CSV加载路径
+
+```cpp
+#include "path_curve.h"
+#include "path_tracker.h"
+
+int main() {
+    // 创建路径对象
+    PathCurve path;
+    
+    // 从CSV文件加载
+    if (path.loadFromCSV("custom_path.csv", true)) {
+        std::cout << "路径加载成功！" << std::endl;
+        std::cout << "路径点数量: " << path.getPathPoints().size() << std::endl;
+        std::cout << "路径长度: " << path.getPathLength() << " m" << std::endl;
+        
+        // 使用加载的路径进行跟踪
+        AGVModel agv(2.0, 1.0, M_PI/4);
+        PathTracker tracker(agv);
+        tracker.setReferencePath(path);
+        
+        // 设置初始状态并生成控制序列
+        const auto& points = path.getPathPoints();
+        AGVModel::State initial(points[0].x, points[0].y, points[0].theta);
+        tracker.setInitialState(initial);
+        tracker.generateControlSequence("pure_pursuit", 0.1, 20.0);
+        
+        // 保存结果
+        tracker.saveTrajectory("output_trajectory.csv");
+    }
+    
+    return 0;
+}
+```
+
+### 示例 2：使用样条插值
+
+```cpp
+#include "path_curve.h"
+
+int main() {
+    PathCurve path;
+    
+    // 定义关键点
+    std::vector<PathPoint> key_points;
+    key_points.push_back(PathPoint(0.0, 0.0));
+    key_points.push_back(PathPoint(5.0, 2.0));
+    key_points.push_back(PathPoint(10.0, 5.0));
+    key_points.push_back(PathPoint(15.0, 3.0));
+    key_points.push_back(PathPoint(20.0, 0.0));
+    
+    // 生成样条曲线（200个点）
+    path.generateSpline(key_points, 200, 0.5);
+    
+    std::cout << "样条曲线生成完成" << std::endl;
+    std::cout << "路径长度: " << path.getPathLength() << " m" << std::endl;
+    
+    // 保存到文件
+    path.saveToCSV("spline_path.csv");
+    
+    return 0;
+}
+```
+
+### 示例 3：手动创建并保存路径
+
+```cpp
+#include "path_curve.h"
+
+int main() {
+    PathCurve path;
+    
+    // 创建路径（例如：贝塞尔曲线）
+    PathPoint p0(0, 0), p1(5, 10), p2(15, 10), p3(20, 0);
+    path.generateCubicBezier(p0, p1, p2, p3, 150);
+    
+    // 保存到CSV
+    if (path.saveToCSV("my_custom_path.csv")) {
+        std::cout << "路径已保存" << std::endl;
+    }
+    
+    return 0;
+}
+```
+
+## CSV 文件格式
+
+### 完整格式示例
+
+```csv
+# Custom Path Data
+# x(m), y(m), theta(rad), kappa(1/m)
+0.0, 0.0, 0.0, 0.0
+1.0, 0.5, 0.1, 0.05
+2.0, 1.2, 0.15, 0.03
+```
+
+### 简化格式示例
+
+```csv
+# Custom Path - Simple Format
+# x(m), y(m)
+0.0, 0.0
+2.0, 1.0
+4.0, 3.0
+6.0, 4.0
+```
+
+## 参数说明
+
+### loadFromCSV 参数
+
+- `filename`: CSV文件路径（相对或绝对路径）
+- `has_header`: 是否包含注释/表头行（默认true）
+
+### generateSpline 参数
+
+- `key_points`: 关键点数组（最少2个点）
+- `num_points`: 生成的总点数（推荐100-500）
+- `tension`: 张力系数（0.0 = 最平滑，1.0 = 最紧密，推荐0.5）
+
+## 常见用例
+
+### 用例 1：地图路径规划
+
+从地图软件导出路径点（CSV格式），直接加载使用：
+
+```cpp
+PathCurve map_path;
+map_path.loadFromCSV("map_waypoints.csv");
+```
+
+### 用例 2：平滑路径优化
+
+将粗糙的路径点用样条插值平滑化：
+
+```cpp
+// 加载原始路径
+PathCurve rough_path;
+rough_path.loadFromCSV("raw_path.csv");
+
+// 提取关键点（每隔10个点取一个）
+std::vector<PathPoint> key_points;
+const auto& points = rough_path.getPathPoints();
+for (size_t i = 0; i < points.size(); i += 10) {
+    key_points.push_back(points[i]);
+}
+
+// 生成平滑路径
+PathCurve smooth_path;
+smooth_path.generateSpline(key_points, 300, 0.3);
+smooth_path.saveToCSV("smooth_path.csv");
+```
+
+### 用例 3：交互式路径定义
+
+通过用户输入定义路径：
+
+```cpp
+std::vector<PathPoint> user_points;
+int n;
+std::cout << "输入路径点数量: ";
+std::cin >> n;
+
+for (int i = 0; i < n; i++) {
+    double x, y;
+    std::cout << "点" << (i+1) << " x: "; std::cin >> x;
+    std::cout << "点" << (i+1) << " y: "; std::cin >> y;
+    user_points.push_back(PathPoint(x, y));
+}
+
+PathCurve user_path;
+user_path.generateSpline(user_points, 200);
+user_path.saveToCSV("user_defined_path.csv");
+```
+
+## 与现有路径类型的对比
+
+| 路径类型 | 定义方式 | 灵活性 | 适用场景 |
+|---------|---------|--------|---------|
+| **直线** | 起点+终点 | 低 | 简单直线移动 |
+| **圆弧** | 圆心+半径+角度 | 低 | 固定半径转弯 |
+| **贝塞尔曲线** | 4个控制点 | 中 | S型曲线 |
+| **CSV加载** | 外部文件 | 高 | 复杂预定义路径 |
+| **样条插值** | 关键点数组 | 极高 | 任意平滑曲线 |
+
+## 调试技巧
+
+### 1. 验证加载的路径
+
+```cpp
+const auto& points = path.getPathPoints();
+for (size_t i = 0; i < points.size(); i += 10) {
+    std::cout << "Point " << i << ": ("
+              << points[i].x << ", " << points[i].y << ")" << std::endl;
+}
+```
+
+### 2. 检查路径连续性
+
+```cpp
+double max_distance = 0.0;
+for (size_t i = 1; i < points.size(); i++) {
+    double dx = points[i].x - points[i-1].x;
+    double dy = points[i].y - points[i-1].y;
+    double dist = sqrt(dx*dx + dy*dy);
+    max_distance = std::max(max_distance, dist);
+}
+std::cout << "最大点间距: " << max_distance << " m" << std::endl;
+```
+
+## 常见问题
+
+**Q: CSV文件格式不正确怎么办？**
+A: 确保每行格式为 `数字, 数字, ...`，使用逗号分隔，可以有注释行（以#开头）。
+
+**Q: 样条插值生成的路径不平滑？**
+A: 尝试增加点数（num_points），或减小tension参数。
+
+**Q: 路径跟踪效果不理想？**
+A: 检查路径曲率（kappa值），确保不超过AGV的最大转向能力。
+
+## 完整示例程序
+
+项目中提供了完整的示例：
+- `examples/custom_path.csv` - 示例CSV路径文件
+- `src/path_curve_custom.cpp` - 实现代码
+
+## 总结
+
+使用自定义路径功能的基本步骤：
+
+1. **准备路径数据** - CSV文件或关键点数组
+2. **创建PathCurve对象** - 使用loadFromCSV或generateSpline
+3. **验证路径** - 检查点数和长度
+4. **应用到跟踪器** - 使用tracker.setReferencePath()
+5. **运行仿真** - 生成控制序列并保存结果
+
+现在你可以摆脱预设曲线的限制，使用任意自定义路径进行AGV路径跟踪！