# 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路径跟踪！