# 自定义路径功能 - 快速开始

## 最简单的使用方式

### 方法 1：从CSV文件加载路径（推荐）

#### 步骤 1：准备CSV文件

创建一个文件 `my_path.csv`：

```csv
# My Custom Path
# x, y
0, 0
2, 1
4, 3
6, 4
8, 4
10, 3
12, 1
14, 0
```

#### 步骤 2：编写代码

```cpp
#include "path_tracker.h"

int main() {
    // 1. 创建并加载路径
    PathCurve path;
    path.loadFromCSV("my_path.csv");
    
    // 2. 创建AGV和跟踪器
    AGVModel agv(1.0, 2.0, M_PI/4);
    PathTracker tracker(agv);
    tracker.setReferencePath(path);
    
    // 3. 运行
    const auto& pts = path.getPathPoints();
    AGVModel::State initial(pts[0].x, pts[0].y, pts[0].theta);
    tracker.setInitialState(initial);
    tracker.generateControlSequence("pure_pursuit", 0.1, 20.0);
    
    // 4. 保存结果
    tracker.saveTrajectory("result.csv");
    
    return 0;
}
```

#### 步骤 3：编译运行

```bash
cd build
cmake --build .
./my_program
```

### 方法 2：使用样条插值

如果你只有几个关键点，想生成平滑曲线：

```cpp
#include "path_curve.h"

int main() {
    PathCurve path;
    
    // 只需要定义几个关键点
    std::vector<PathPoint> keypoints = {
        PathPoint(0, 0),
        PathPoint(5, 3),
        PathPoint(10, 2),
        PathPoint(15, 0)
    };
    
    // 自动生成200个平滑点
    path.generateSpline(keypoints, 200, 0.5);
    
    // 保存用于可视化或后续使用
    path.saveToCSV("smooth_path.csv");
    
    return 0;
}
```

## 完整工作流示例

### 场景：仓库AGV路径规划

```cpp
#include "path_tracker.h"
#include <iostream>

int main() {
    std::cout << "=== 仓库AGV路径跟踪系统 ===" << std::endl;
    
    // 第1步：定义仓库路径关键点
    std::vector<PathPoint> warehouse_waypoints = {
        PathPoint(0, 0),      // 起点：充电站
        PathPoint(5, 0),      // 货架A
        PathPoint(5, 10),     // 货架B
        PathPoint(15, 10),    // 货架C
        PathPoint(15, 5),     // 出货口
        PathPoint(20, 0)      // 终点：卸货区
    };
    
    // 第2步：生成平滑路径
    PathCurve path;
    path.generateSpline(warehouse_waypoints, 300, 0.4);
    
    std::cout << "路径生成: " << path.getPathPoints().size() 
              << " 点, 长度 " << path.getPathLength() << " m" << std::endl;
    
    // 第3步：保存路径用于记录
    path.saveToCSV("warehouse_path.csv");
    
    // 第4步：配置AGV参数
    AGVModel agv(
        1.5,       // 最大速度 1.5 m/s
        1.2,       // 轴距 1.2 m
        M_PI/3     // 最大转向角 60度
    );
    
    // 第5步：执行路径跟踪
    PathTracker tracker(agv);
    tracker.setReferencePath(path);
    
    const auto& pts = path.getPathPoints();
    AGVModel::State start(pts[0].x, pts[0].y, pts[0].theta);
    tracker.setInitialState(start);
    
    // 使用Pure Pursuit算法
    if (tracker.generateControlSequence("pure_pursuit", 0.1, 30.0)) {
        std::cout << "跟踪成功！" << std::endl;
        
        // 保存结果
        tracker.saveTrajectory("warehouse_trajectory.csv");
        tracker.saveControlSequence("warehouse_control.csv");
        
        std::cout << "结果已保存，可使用 python visualize.py 可视化" << std::endl;
    }
    
    return 0;
}
```

## 三种路径定义方式对比

| 方式 | 代码行数 | 适用场景 | 优点 |
|-----|---------|---------|-----|
| **CSV加载** | 2行 | 已知完整路径 | 最简单，易修改 |
| **样条插值** | 5-10行 | 已知关键点 | 平滑，点数可控 |
| **预设曲线** | 3-5行 | 简单几何形状 | 参数化，精确 |

## 常用代码片段

### 检查路径是否有效

```cpp
if (path.getPathPoints().size() < 2) {
    std::cerr << "路径点太少！" << std::endl;
    return -1;
}

if (path.getPathLength() < 1.0) {
    std::cerr << "路径太短！" << std::endl;
    return -1;
}
```

### 打印路径信息

```cpp
const auto& points = path.getPathPoints();
std::cout << "路径信息:" << std::endl;
std::cout << "  点数: " << points.size() << std::endl;
std::cout << "  长度: " << path.getPathLength() << " m" << std::endl;
std::cout << "  起点: (" << points.front().x << ", " 
          << points.front().y << ")" << std::endl;
std::cout << "  终点: (" << points.back().x << ", " 
          << points.back().y << ")" << std::endl;
```

### 路径可视化（使用Python）

```python
import pandas as pd
import matplotlib.pyplot as plt

# 读取CSV
path = pd.read_csv('my_path.csv', comment='#')

# 绘制
plt.figure(figsize=(10, 6))
plt.plot(path.iloc[:, 0], path.iloc[:, 1], 'b-', linewidth=2)
plt.scatter(path.iloc[:, 0], path.iloc[:, 1], c='red', s=50)
plt.grid(True)
plt.axis('equal')
plt.xlabel('X (m)')
plt.ylabel('Y (m)')
plt.title('Custom Path')
plt.show()
```

## 故障排除

### 问题 1：CSV加载失败

```
Error: Cannot open file my_path.csv
```

**解决方案**：
- 检查文件路径是否正确
- 使用绝对路径：`path.loadFromCSV("C:/full/path/to/file.csv")`

### 问题 2：样条曲线不平滑

```cpp
// 尝试增加点数
path.generateSpline(keypoints, 500, 0.5);  // 增加到500点

// 或减小tension参数
path.generateSpline(keypoints, 200, 0.2);  // 更平滑
```

### 问题 3：编译错误 "loadFromCSV未定义"

需要先安装自定义路径功能：

```bash
bash install_custom_path.sh
```

或手动添加到CMakeLists.txt：
```cmake
set(SOURCES
    ...
    src/path_curve_custom.cpp  # 添加这行
)
```

## 下一步

- 阅读完整文档：`CUSTOM_PATH_GUIDE.md`
- 查看示例文件：`examples/custom_path.csv`
- 运行现有demo：`./build/agv_demo`
- 尝试不同的控制算法：pure_pursuit, stanley, mpc

## 获取帮助

如有问题，请查看：
1. 完整使用指南：`CUSTOM_PATH_GUIDE.md`
2. 原有功能文档：`README.md`, `QUICKSTART.md`
3. 代码示例：`examples/` 目录