ROS2 Navigation2 概览

Navigation2 (简称NAV2) 是ROS2中用于实现移动机器人自主导航的核心功能包套件。它继承并极大地扩展了ROS1 Navigation Stack的功能，采用了更加模块化、可组合的设计理念。其根本目标是让机器人能够在复杂的环境中，安全、高效地从一个点移动到另一个点。

为了更好地理解，您可以将NAV2想象成一个智能的“机器人司机”，它需要：

1. 一张地图 (Map)：了解周围环境的静态布局。
2. 一个定位系统 (Localization)：知道自己当前在地图的哪个位置。
3. 一个路径规划器 (Path Planner)：规划出一条从起点到终点的全局路径。
4. 一个实时避障器 (Local Planner/Controller)：在沿着规划路径行进时，能够实时躲避突然出现的障碍物。
5. 行为树 (Behavior Tree)：作为“大脑”，协调以上所有功能，并处理导航过程中的各种状态和异常情况。
   

---

1. 模块组成

以下是Navigation2的核心模块清单，以表格形式呈现，这与最新的NAV2官方架构图是完全对应的。

模块名称 (Package/Node Name)	主要功能	主要输入 (Input)	主要输出 (Output)
nav2_bringup	启动与配置	nav2_params.yaml (配置文件)	启动所有NAV2节点
nav2_map_server	地图服务器	map.yaml (地图元数据), map.pgm (地图图像)	/map (Topic, 静态地图)
nav2_amcl	定位模块 (AMCL)	/scan (Topic, 激光雷达数据), /map (Topic, 地图), /tf (TF, 里程计坐标变换)	/particle_cloud (Topic, 粒子云), /amcl_pose (Topic, 机器人位姿), map -> odom (TF, 坐标变换)
nav2_bt_navigator	行为树导航器	goal_pose (Action, 目标点), /tf (TF, 机器人位姿)	/cmd_vel (Topic, 速度指令), MapsToPose (Action, 结果反馈)
nav2_planner	全局路径规划器	goal_pose (Action, 目标点), /map (Topic, 地图), /global_costmap (Topic, 全局代价地图)	/plan (Topic, 全局路径)
nav2_controller	局部路径规划/控制器	/plan (Topic, 全局路径), /odom (Topic, 里程计信息), /local_costmap (Topic, 局部代价地图)	/cmd_vel (Topic, 速度指令)
nav2_costmap_2d	代价地图	/scan, /points, etc. (传感器数据), /map (静态地图)	/global_costmap, /local_costmap (Topic, 代价地图)
nav2_behaviors	行为服务器	(由行为树调用)	(向行为树返回成功/失败)
nav2_lifecycle_manager	生命周期管理器	(无)	管理所有NAV2节点的生命周期状态 (Activating, Deactivating, etc.)

---

2. 运行流程讲解 (结合实际导航任务)

让我们以一个常见的场景为例：“在一个办公环境中，让机器人从A点（前台）移动到B点（会议室）。”

阶段一：启动与初始化

1. 启动系统: 用户在终端执行 ros2 launch nav2_bringup bringup_launch.py。
2. 加载配置: nav2_bringup 包会读取 nav2_params.yaml 文件，这里面定义了要使用哪个定位算法、哪个全局规划器、哪个局部控制器，以及代价地图的参数等。
3. 启动节点: nav2_lifecycle_manager (生命周期管理器) 会按照预设顺序，依次启动并激活 (Activate) 所有核心节点，如 map_server, amcl, bt_navigator, planner, controller等。
4. 发布地图: nav2_map_server 读取预先制作好的办公室地图文件 (office_map.yaml 和 office_map.pgm)，并通过 /map topic 将其发布出去。
5. 初始化定位:
   • nav2_amcl (自适应蒙特卡洛定位) 节点订阅 /map topic 和来自激光雷达的 /scan topic。
   • 此时，机器人通常需要一个初始位姿 (Initial Pose)。用户可以通过RVIZ2等工具在地图上标记机器人的大概位置和朝向。
   • amcl 根据初始位姿撒下一片“粒子云” (/particle_cloud)。每一个粒子都代表一种机器人可能存在的位姿。
   • 随着机器人在原地旋转或小范围移动，amcl 不断用激光数据与地图进行比对，淘汰掉那些与观测不符的粒子，最终粒子云会收敛到机器人的真实位置附近。
   • amcl 会持续发布从 map 坐标系到 odom 坐标系的TF变换，这样就将机器人精确地“锚定”在了地图上。

至此，准备工作完成。机器人已经知道了地图长什么样，也知道了自己在哪。它已经准备好接收任务了。

阶段二：执行导航任务

这个阶段的核心是 nav2_bt_navigator (行为树导航器)，它像一个总指挥，调用其他模块来完成任务。

1. 接收目标: 用户通过RVIZ2或命令行，发布一个目标点 (Goal Pose)，即会议室的坐标。这个目标会被发送给 nav2_bt_navigator 的 MapsToPose Action Server。

2. 行为树开始执行: bt_navigator 内部的导航行为树 (BT) 开始从根节点运行。一个简化的典型流程如下：

   • BT节点 1: ComputePathToPose (计算路径)

     • 触发: bt_navigator 调用 nav2_planner (全局规划器)。
     • 数据流: nav2_planner 获取机器人的当前位置 (通过TF) 和目标点位置，同时参考 nav2_costmap_2d 发布的 /global_costmap (全局代价地图)。全局代价地图是在静态地图上加入了“膨胀区”（离墙壁太近会有惩罚）等信息，确保规划出的路径不会贴着墙走。
     • 规划算法: planner 内部的算法（默认为 NavFn 或更现代的 SmacPlanner）在代价地图上执行类似A*的搜索，找到一条从起点到终点的最优路径。
     • 输出: planner 将计算出的路径以 nav_msgs/Path 的形式发布到 /plan topic。
     • 返回: 如果成功找到路径，该BT节点返回 SUCCESS；否则返回 FAILURE。

   • BT节点 2: FollowPath (跟随路径)

     • 触发: 如果上一步成功，行为树会执行这个节点。bt_navigator 开始调用 nav2_controller (局部控制器)。
     • 数据流: nav2_controller 订阅 /plan topic 获取要遵循的全局路径。同时，它密切关注 nav2_costmap_2d 发布的 /local_costmap (局部代价地图)。局部代价地图是一个以机器人为中心的小范围地图，它会实时地将激光雷达等传感器检测到的动态障碍物（如走过的同事）也包含进来。
     • 控制算法: controller 内部的算法（如 DWB 或 MPPI）会做几件事：
       1. 从全局路径上取一小段作为当前参考。
       2. 在机器人前方模拟多条可能的轨迹。
       3. 根据“是否接近参考路径”、“是否会碰到局部代价地图上的障碍物”、“速度是否平滑”等标准对这些轨迹进行打分。
       4. 选择得分最高的轨迹，并将其转换为给机器人底盘的 /cmd_vel (速度指令，包括线速度和角速度)。
     • 循环执行: 这个过程以很高的频率（例如10-20Hz）不断重复。机器人每前进一步，controller 就会根据最新的传感器数据重新评估和发出新的速度指令。这使得机器人能够平滑地沿着全局路径前进，并能灵活地绕开突然出现的障碍物。
     • 返回: 只要机器人还在正常跟随路径，该节点就处于 RUNNING 状态。

3. 任务完成与异常处理:

   • 成功: 当 controller 判断机器人已经到达 /plan 上的最后一个点附近时，FollowPath 节点返回 SUCCESS。整个行为树执行完毕，bt_navigator 向最初的请求者（如RVIZ2）返回成功的结果。
   • 失败与恢复 (Recovery Behaviors):
     • 情景1: 规划失败: 如果 ComputePathToPose 返回 FAILURE (例如，目标点在墙里)，行为树可能会直接中止任务并报告失败。
     • 情景2: 跟随失败: 如果在 FollowPath 过程中，机器人被卡住了（例如，轮子打滑，或者被障碍物完全堵死），controller 会上报失败。此时，行为树会进入恢复行为 (Recovery Behaviors) 分支。
     • 恢复节点: 行为树会依次尝试执行预设的恢复策略，例如：
       • ClearCostmap: 清除代价地图上的障碍物信息（可能有些障碍物已经消失了）。
       • Spin: 原地旋转360度，让传感器重新观察周围环境。
       • Wait: 等待一段时间。
     • 重试: 每执行完一个恢复行为后，行为树会尝试重新规划路径 (ComputePathToPose) 和跟随路径 (FollowPath)。如果重试成功，导航继续；如果所有恢复行为都尝试过后依然失败，则整个导航任务宣告失败。