agv-control-slam/docs/custom_path/QUICKSTART_CUSTOM_PATH.md

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

最简单的使用方式

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

步骤 1：准备CSV文件

创建一个文件 my_path.csv：

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

步骤 2：编写代码

#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：编译运行

cd build
cmake --build .
./my_program

方法 2：使用样条插值

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

#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路径规划

#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行	简单几何形状	参数化，精确

常用代码片段

检查路径是否有效

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

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

打印路径信息

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）

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：样条曲线不平滑

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

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

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

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

bash install_custom_path.sh

或手动添加到CMakeLists.txt：

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/ 目录