34. 多点导航、路径跟踪与自主探索¶
第 33 章介绍的是“给一个目标点，机器人规划并到达”。实际任务往往不是只去一个点：机器人可能要依次巡检几个位置，也可能要严格跟随一条已经给出的路线，或者在未知区域中自己寻找下一块值得探索的区域。本章讨论这些任务层功能。它们建立在第 33 章的地图、定位、TF、代价地图和底盘控制之上，不需要另起一套传感器驱动或运动控制链。
34.1 多点导航：把多个目标交给 Nav2¶
多点导航不是把多个目标点拼成一条字符串，而是由上层任务节点按顺序发送一组 Nav2 动作目标。常见接口有：
| 任务 | 常见动作接口 | 说明 |
|---|---|---|
| 到达一个目标 | NavigateToPose |
发送一个位置和朝向 |
| 依次经过多个目标 | NavigateThroughPoses |
一次发送多个姿态，由 Nav2 依次规划和到达 |
| 按航点执行任务 | FollowWaypoints |
在航点之间导航，并可在每个航点触发等待或任务回调 |
上层程序通常维护目标列表、当前索引和失败策略：到达一个目标后再发送下一个；某个点失败时可以重试、跳过或停止。每个目标都必须使用 Nav2 能够转换到的坐标系（通常是 map），并给出合理的朝向。目标点太密、落在障碍物内或超出地图范围，都会让规划器无法生成路径。
一次多点任务的数据流可以概括为：
目标列表 ──动作接口──► bt_navigator
│
规划、跟踪、恢复
│
/cmd_vel
▼
底盘
34.2 路径跟踪：已经有路线时怎样走¶
路径跟踪的输入不是单个终点，而是 nav_msgs/msg/Path：其中包含一串带坐标和朝向的 PoseStamped。路径可以由人工示教、规划算法、地图上的航线或外部程序生成。控制器逐周期比较“机器人当前位姿”和“路径上应该到达的点”，输出短时的 geometry_msgs/msg/Twist，再由底盘驱动发布到 STM32。
使用路径前要检查三件事：
Path.header.frame_id是否是map或系统实际使用的全局坐标系；- 路径点是否按行驶方向排列，点间距是否适合控制器；
- 路径是否经过代价地图允许的区域，路径跟踪控制器是否支持当前底盘的运动学约束。
路径跟踪只负责“沿给定路线走”，不会自动证明这条路线安全。实时障碍物仍应进入局部代价地图；若路径被障碍物挡住，控制器应减速、停止或请求重新规划。
34.3 自主探索：从未知区域选择下一个目标¶
自主探索通常使用已经部分建好的地图，在已知区域和未知区域的边界寻找候选目标。这个边界称为 frontier。探索节点会反复执行以下过程：
读取当前地图 → 找到未知区域边界 → 过滤不可达/危险目标
→ 选择一个目标 → 调用 Nav2 到达 → 地图更新后再次选择
探索节点本身一般不负责底盘控制，而是调用 NavigateToPose 或 NavigateThroughPoses。停止条件应明确，例如未知区域低于某个面积、连续多次找不到可达 frontier、运行时间到达上限或电量过低。没有停止条件的探索程序可能在同一区域反复导航。
34.4 这些算法有什么区别¶
多点导航、路径跟踪和自主探索描述的是任务方式；“规划器插件”描述的是 Nav2 怎样计算从起点到目标的路径，两者不是同一层概念。
快速扩展随机树（Rapidly-Exploring Random Tree, RRT）是一类通过随机采样逐步扩展搜索树的规划方法，RRT* 则在搜索过程中继续优化已有路径。
| 方法 | 需要什么输入 | 主要特点 | 适合场景 |
|---|---|---|---|
| 栅格搜索（如 NavFn、Smac 2D） | 静态/动态代价地图、起点和目标 | 在网格上搜索可通行路径，结果稳定、易解释 | 普通二维室内地图 |
| 带运动学约束的搜索（如 Smac Hybrid-A*） | 代价地图、车辆最小转弯半径等 | 同时考虑朝向和转弯能力 | 阿克曼或不能横移的底盘 |
| 采样类规划（如 RRT/RRT*） | 可通行空间、碰撞检测和起终点 | 在连续空间随机采样扩展，适合复杂约束；结果可能随随机种子变化 | 复杂几何或高维规划问题 |
| 前沿探索 | 带未知区域的地图、可达性评估 | 不以固定终点为主，而是不断选择未知区域边界 | 自动覆盖未知环境 |
更换规划器时，不只是改一个算法名称，还要核对插件参数、代价地图分辨率、机器人 footprint、速度/加速度限制、目标朝向和控制器是否能跟踪新路径。先在仿真或低速空旷区域验证，再接入真实任务。
34.5 运行时怎样确认任务真的在工作¶
只看 RViz2 中的线条不足以判断任务完成。可以结合动作、路径和速度话题检查：
ros2 action list
ros2 topic echo --once /plan
ros2 topic echo --once /local_plan
ros2 topic echo --once /cmd_vel
ros2 topic info /cmd_vel --verbose
实际名称可能带有命名空间，应以 ros2 topic list 和 ros2 action list 的结果为准。多点任务还应记录当前目标编号、动作反馈和失败原因；主动取消任务后，确认 /cmd_vel 不再持续输出非零速度。
常见问题包括：目标顺序与列表索引不一致、路径的 frame_id 错误、路径点落在障碍物内、探索节点重复选择同一 frontier，以及取消动作后任务线程仍在发送新目标。排查时先确认 Nav2 基础链和 TF 正常，再检查任务节点自己的状态机。
本章代码：路径跟踪
仓库目录：ros2/navigation/path-follow/wheeltec_path_follow
可先查看 package.xml、launch/follow_path.launch.py 和 scripts/follow_path.py，确认路径消息如何产生以及动作接口如何调用。该示例只说明任务层程序的入口，不替代实机的急停和低速测试。