258 lines
5.8 KiB
Markdown
258 lines
5.8 KiB
Markdown
# 自定义路径功能 - 快速开始
|
||
|
||
## 最简单的使用方式
|
||
|
||
### 方法 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/` 目录
|