33. Nav2 导航¶
第 32 章已经完成地图加载和机器人定位。此时系统能够回答“机器人现在位于地图中的哪里”，接下来才是让机器人根据目标位置自动规划路径、避开障碍并控制底盘运动。
Navigation2（Nav2）是 ROS 2 中常用的一套导航框架，通常通过 navigation2、nav2_bringup 等软件包安装。它不是 ROS 2 自动提供的一个单独节点，也不是一个程序包办全部工作，而是由多个节点、服务器和可替换的算法插件共同完成导航。
33.1 Nav2 接收什么，又输出什么¶
Nav2 不直接读取雷达串口、编码器脉冲或地图文件中的像素。前面章节的驱动、里程计、TF、地图服务器和定位节点先把底层数据变成 ROS 2 接口，Nav2 再使用这些接口完成导航：
地图 + 当前位姿 + 实时障碍物 + 里程计 + 导航目标
↓
Nav2
↓
路径 + 速度指令 /cmd_vel
↓
底盘运动
主要输入及其来源如下：
| Nav2 需要的信息 | 常见 ROS 2 表达 | 通常由谁提供 | Nav2 用它做什么 |
|---|---|---|---|
| 已有地图 | /map，nav_msgs/msg/OccupancyGrid |
地图服务器 | 判断大范围内哪里可通行、哪里有静态障碍 |
| 机器人当前位置 | map → odom → base_link TF 链 |
定位节点、底盘或里程计融合节点 | 确定路径起点，并跟踪机器人是否偏离路径 |
| 实时障碍物 | /scan 或 PointCloud2 |
雷达、深度相机等驱动节点 | 更新机器人附近的障碍物，避开临时出现的人或物体 |
| 机器人运动状态 | /odom，nav_msgs/msg/Odometry |
底盘或里程计融合节点 | 让控制器知道机器人当前的速度和短时运动变化 |
| 机器人外形和运动限制 | footprint、速度、加速度、最小转弯半径等参数 | URDF 和 Nav2 参数文件 | 判断车体能否通过，并生成底盘能够执行的运动 |
| 导航目标 | NavigateToPose 等动作接口 |
RViz2 或上层任务程序 | 指定希望机器人到达的位置和朝向 |
地图和实时雷达不能互相替代。/map 记录建图时已经存在的墙壁和固定物体，实时传感器则负责发现地图中没有记录的新障碍物。定位确定机器人在整张地图中的位置，里程计提供连续、平滑的短时运动估计。
导航目标通过 Nav2 的动作接口发送，而不是直接向 /cmd_vel 写一个“去某处”的字符串。Nav2 最主要的控制输出是 /cmd_vel，底盘驱动订阅它后才会控制电机。直接发布 /cmd_vel 只能让机器人运动，不能替代地图规划、障碍物检查和到达判断。
33.2 Nav2 内部怎样实现一次导航¶
Nav2 的 Launch 文件会启动一组 ROS 2 节点。节点之间通过话题、动作、服务和 TF2 交换数据；具体算法则由服务器加载插件。因此，“Nav2”不是一个黑盒程序，而是一条可以逐段检查的 ROS 2 处理链：
RViz2 或上层程序
│ NavigateToPose 动作
▼
行为树导航器 ──调用──► 规划器服务器 ──发布──► 全局路径
│ ▲
│ │ 全局代价地图
│ │
└──调用──► 控制器服务器 ◄── 局部代价地图 ◄── 雷达 / 点云
│
▼
/cmd_vel
│
▼
底盘驱动与 STM32
各部分的实现方式如下：
| 部分 | 在 ROS 2 中通常是什么 | 它怎样工作 |
|---|---|---|
| 地图服务器 | map_server 生命周期节点 |
读取 .yaml 和 .pgm，把地图封装为 OccupancyGrid，发布 /map |
| 定位节点 | 例如 amcl 节点 |
订阅 /scan、/map 和里程计，通过粒子滤波估计 map → odom |
| 代价地图 | 全局和局部 costmap_2d 服务器 |
把地图、传感器观测、footprint 和膨胀半径转换成带代价的网格 |
| 规划器服务器 | planner_server 生命周期节点 |
接收目标和机器人位姿，按参数加载一个规划器插件，输出 nav_msgs/msg/Path |
| 控制器服务器 | controller_server 生命周期节点 |
读取路径、局部代价地图、TF 和 /odom，按控制器插件计算 Twist |
| 行为树导航器 | bt_navigator 生命周期节点 |
执行 XML 行为树，依次调用规划、跟踪、取消和恢复动作 |
| 恢复行为服务器 | behavior_server 生命周期节点 |
在规划或控制失败时执行旋转、后退、等待等动作 |
| 生命周期管理器 | lifecycle_manager 节点 |
让上述节点按顺序经历 configure、activate 等状态，避免未准备好的节点直接工作 |
一次导航可以按下面的顺序理解：
- RViz2 把目标位置封装为 Nav2 动作目标，发送给
bt_navigator； - 行为树先要求规划器服务器计算全局路径；
- 规划器插件读取全局代价地图，在可通行网格中搜索路径并返回
Path； - 控制器服务器周期性读取这条路径、机器人当前 TF、
/odom和局部代价地图，计算一条短时速度指令； /cmd_vel传给前面章节的底盘驱动，再由串口发送给 STM32；- 雷达的新数据进入局部代价地图，控制器据此减速、绕行或停止；
- 如果路径失效或控制器连续失败，行为树会请求重新规划，必要时调用恢复行为；
- 机器人满足位置和朝向容差后，动作返回成功并发送零速度。
全局规划器关注“在整张地图上从哪里绕过去”，控制器关注“下一小段怎样安全地走”。例如地图上有一条走廊，局部雷达突然发现箱子挡住了路：局部代价地图先把箱子写入网格，控制器会减速、停止或绕行；如果找不到可执行的局部轨迹，行为树才会触发重新规划或恢复动作。
33.3 启动前先确认接口¶
第 32 章的定位结果必须先稳定，再启动 Nav2。至少确认：
/map正在发布，地图分辨率和实际场地一致；map → odom → base_link → laser坐标链完整，且每段 TF 只有一个发布者；- 定位节点提供连续的机器人位姿；
- 雷达或其他障碍物传感器在局部代价地图中可见；
- 底盘能够接收
/cmd_vel并在取消目标或失败后停止； - 机器人 footprint、最大速度、雷达盲区和制动距离已经按实物设置。
可以先查看节点、话题和动作接口：
ros2 node list
ros2 topic list -t
ros2 action list -t
ros2 topic info /cmd_vel --verbose
ros2 run tf2_ros tf2_echo map base_link
实际系统的话题名可能带有命名空间，例如 /robot1/cmd_vel。不要只因为 Launch 文件成功退出就判断导航已经可用，要确认生命周期节点已经进入 active，并且确实订阅了地图、定位和传感器输入。
33.4 第一次发送导航目标¶
第一次测试应选择机器人前方约 0.5～1 m 的空旷目标，并准备急停。可以在 RViz2 中使用 Nav2 的目标工具发送目标，也可以由程序调用导航动作接口。观察顺序如下：
- 目标是否出现在
map坐标系中； - 全局代价地图是否把目标所在区域判定为可通行；
- 全局规划器是否生成路径；
- 局部代价地图是否显示实时雷达障碍物；
- 控制器是否输出合理的
/cmd_vel； - 机器人到达、取消或失败后是否可靠停止。
如果机器人还没有通过短距离测试，不要直接发送跨房间目标。先用低速、短距离和空旷路线确认定位、路径、控制和急停链路，再逐步增加距离和障碍物。
33.5 代价地图和 footprint 为什么重要¶
Nav2 不只看机器人中心点。代价地图还要根据机器人的外形判断车体是否会碰到障碍物。footprint 过小，规划器可能认为机器人能通过实际无法通过的缝隙；footprint 过大，则可能把本来可通行的区域判成不可通行。
雷达安装高度、盲区和障碍物高度也会影响代价地图。例如雷达看不到车体附近的低矮障碍，局部代价地图就不会自动知道那里有障碍；这不是规划器参数可以完全补救的问题，需要增加传感器、调整安装或设置安全膨胀范围。
33.6 导航失败时怎样定位问题¶
排查顺序应从输入到输出逐层进行：
地图与定位 → TF 与时间 → 传感器障碍物 → 代价地图
→ 全局路径 → 局部控制 → 底盘执行
| 现象 | 优先检查 | 可能原因 |
|---|---|---|
| 没有全局路径 | 目标位姿、地图、全局代价地图 | 目标在未知区或障碍物内，地图原点或坐标不一致 |
| 有全局路径但没有速度 | 局部代价地图、控制器、生命周期状态 | 机器人周围被判定为不可通行，控制器未 active |
| 机器人原地振荡 | footprint、目标朝向、最小速度、里程计 | 车体尺寸或速度方向配置错误 |
| 轨迹穿过障碍 | 雷达数据、传感器 TF、膨胀半径 | 障碍物没有进入局部代价地图，安装坐标错误 |
| 到达目标后仍不停止 | 到达容差、控制器和底盘停止链 | 目标判定或速度复用节点配置错误 |
| 位姿突然跳动 | map → odom 发布者、时间戳、定位节点 |
同一段 TF 重复发布或定位失去匹配 |
只有当地图、定位、TF 和传感器输入都正常时，才调整规划器或控制器参数。一次只改一个参数，并记录修改前后的路径、速度和日志。
教材代码资源
本节使用 ros2/navigation/nav2-waypoint-cycle 中的 package.xml 与 nav2_waypoint_cycle/waypoint_cycle.py，用于静态追踪航点动作客户端和循环入口。源码固定在 62aacaed。验收：能说明航点动作客户端与循环逻辑；边界：运行前核对 Nav2 发行版。