ROS2导航框架详解：Nav2架构、Behavior Tree、生命周期节点、插件机制

好，咱们直接进入正题。上一章我跟你聊了聊人形机器人导航的整体图景，这一章咱们得把 ROS2 导航框架——也就是 Nav2——的底裤扒开看看。说白了，Nav2 就是 ROS2 时代官方钦定的导航解决方案。你如果还在用 ROS1 的 move_base，那真该升级了。

我刚开始从 ROS1 切到 ROS2 时，第一感觉就是：Nav2 怎么这么复杂？又是 Behavior Tree，又是生命周期节点，还有一堆插件。但用熟了之后你会发现，这套设计其实非常优雅。它把导航这个复杂任务拆成了一个个可插拔的模块，你甚至可以像搭积木一样定制自己的导航系统。

2.1 Nav2 整体架构

先看一张图，这是我手绘的 Nav2 核心架构。你注意看，它分成了三层：行为层、控制层和感知层。

你看，行为层在最上面，它通过 Behavior Tree 来编排整个导航流程。控制层负责具体的路径规划和速度控制。感知层则提供地图和代价地图数据。这三层之间通过 ROS2 的 action 和 topic 通信。

我个人习惯把 Nav2 理解成一个"导航操作系统"——它提供了基础设施，但具体怎么用，完全取决于你。比如人形机器人，它的运动模型和轮式机器人完全不同，所以你需要定制控制层的插件。

2.2 Behavior Tree：导航的"大脑"

Behavior Tree（行为树）是 Nav2 最核心的设计之一。我刚开始接触时也觉得这东西有点玄乎，但说白了，它就是一套可组合的决策逻辑。

你想想看，导航过程中机器人需要做一系列决策：先定位，再规划路径，然后开始移动，遇到障碍物要避让，走不通要重新规划……如果用传统的状态机来写，代码会变得非常臃肿。Behavior Tree 用树状结构把这些逻辑组织起来，每个节点只负责一件事。

Nav2 默认提供了一套 Behavior Tree，路径通常在 nav2_bt_navigator/behavior_trees/ 下。核心的树叫 navigate_to_pose.xml，它定义了从接收目标点到到达目标点的完整流程。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="root_sequence">
      <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      <FollowPath path="{path}" controller_id="FollowPath"/>
    </Sequence>
  </BehaviorTree>
</root>

这段 XML 看起来简单，但背后大有文章。Sequence 节点表示顺序执行，先规划路径，再跟踪路径。如果规划失败，整个序列就失败，导航终止。这就是 Behavior Tree 的"短路"特性——一旦某个节点失败，父节点会立即处理，不会继续执行无意义的操作。

我遇到过一个问题：人形机器人在狭窄走廊里，全局规划出来的路径没问题，但局部控制时总是撞墙。后来发现是 Behavior Tree 里缺少了"重规划"的逻辑。默认的树只有一次规划，一旦路径被阻塞，机器人就卡死了。解决方案是加上一个 RecoveryNode，当路径跟踪失败时，先执行旋转或后退动作，然后重新规划。

2.3 生命周期节点：让导航"活"起来

生命周期节点（LifecycleNode）是 ROS2 引入的新概念。在 ROS1 里，节点要么启动，要么关闭，没有中间状态。但在导航系统中，很多节点需要按顺序启动：先加载地图，再启动代价地图，最后才能开始规划路径。

生命周期节点定义了四个状态：

状态	说明	触发动作
Unconfigured	节点已创建，但未初始化	创建时自动进入
Inactive	节点已配置，但未激活	调用 configure()
Active	节点正常运行	调用 activate()
Finalized	节点已销毁	调用 cleanup() 或 shutdown()

你可能会问：这有什么用？我举个例子。在 Nav2 启动时，map_server 先进入 Inactive 状态，加载地图文件。等地图加载完成后，costmap_2d 节点才进入 Active 状态，开始发布代价地图。如果地图加载失败，map_server 会保持在 Unconfigured 状态，其他节点也不会启动。这就避免了"地图还没加载完，机器人就开始规划"的尴尬。

2.4 插件机制：Nav2 的"可插拔"灵魂

Nav2 的插件机制，说白了就是面向接口编程。每个核心功能都定义了一个抽象接口，你可以实现自己的插件来替换默认实现。

举个例子，全局路径规划器。Nav2 默认提供了 NavFnPlanner 和 SmacPlanner。但如果你做人形机器人，可能需要考虑步态约束——比如机器人不能走太陡的坡，或者需要避开某些地形。这时候你就可以写一个自定义的全局规划器插件。

插件需要继承基类并实现接口：

#include "nav2_core/global_planner.hpp"

class HumanoidPlanner : public nav2_core::GlobalPlanner {
public:
  void configure(...) override;
  void cleanup() override;
  void activate() override;
  void deactivate() override;
  nav_msgs::msg::Path createPlan(...) override;
};

然后在 nav2_params.yaml 里注册：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "humanoid_nav/HumanoidPlanner"

我建议你仔细研究一下 Nav2 的插件体系。它支持以下几种插件类型：

• 全局规划器：计算从起点到终点的全局路径
• 局部规划器：根据全局路径和实时传感器数据生成速度指令
• 代价地图层：处理不同来源的障碍物数据（激光、深度相机、语义信息等）
• 路径平滑器：对规划出的路径进行后处理，使其更平滑
• 行为树节点：自定义的 BT 动作节点

2.5 各组件之间的协作流程

最后，咱们把上面这些概念串起来，看看一次完整的导航任务是怎么执行的。

1. 任务发起：用户通过 NavigateToPose action 发送目标点
2. 行为树启动：BtNavigator 加载 Behavior Tree，开始执行根节点
3. 全局规划：ComputePathToPose 节点调用 PlannerServer，生成全局路径
4. 路径跟踪：FollowPath 节点调用 ControllerServer，生成速度指令
5. 实时避障：ControllerServer 实时读取局部代价地图，避开动态障碍物
6. 异常处理：如果路径被阻塞，Behavior Tree 中的 RecoveryNode 触发重规划或旋转
7. 任务完成：到达目标点，Behavior Tree 返回 SUCCESS，action 返回结果

你看，整个流程环环相扣。任何一个环节出问题，导航就会失败。所以调参时一定要分层调试：先确保代价地图正确，再测试全局规划，最后才跑完整流程。