Nav2 行为树：把失败策略写在决策层

1. 空转到没电，不是控制器不够聪明

机器人贴障原地抖、电池从 80% 掉到 20%，bt_navigator 日志里 FollowPath 一直是 RUNNING——没有 progress 失败、没有 recovery 触发、没有向上层报告。团队拧了三天 FollowPath 的 critic 权重；根因是失败策略不在决策层。Nav2 的行为树（BT）决定：何时重规划、何时 spin/backup/clear、何时放弃。把智慧全塞进控制器参数，BT 缺分支，现场只能空转。

2. BT 在 Nav2 栈里的位置

bt_navigator 加载 XML（默认 navigate_to_pose_w_replanning_and_recovery.xml），通过 BehaviorTree.CPP 驱动 Nav2 插件节点：ComputePathToPose、FollowPath、ClearEntireCostmap、Spin、Wait 等。BT 管策略（失败怎么办），控制器管运动学（怎么跟路径）。两者搅在一个 PR 里，无法证伪。

xml

<RecoveryNode number_of_retries="6" name="NavigateRecovery">
  <PipelineSequence name="NavigateWithReplanning">
    <RateController hz="1.0">
      <RecoveryNode number_of_retries="1" name="ComputePathToPose">
        <ComputePathToPose goal="{goal}" path="{path}"/>
        <ClearEntireCostmap name="ClearGlobalCostmap" service_name="global_costmap/clear_entirely_local_costmap"/>
      </RecoveryNode>
    </RateController>
    <RecoveryNode number_of_retries="1" name="FollowPath">
      <FollowPath path="{path}"/>
      <ClearEntireCostmap name="ClearLocalCostmap" service_name="local_costmap/clear_entirely_local_costmap"/>
    </RecoveryNode>
  </PipelineSequence>
  <ReactiveFallback name="RecoveryFallback">
    <GoalUpdated/>
    <RoundRobin name="RecoveryActions">
      <Sequence name="ClearingActions">
        <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_local_costmap"/>
      </Sequence>
      <Spin spin_dist="1.57"/>
      <Wait wait_duration="5"/>
      <BackUp backup_dist="0.30" backup_speed="0.05"/>
    </RoundRobin>
  </ReactiveFallback>
</RecoveryNode>

3. 节点语义与可观察条件

每个 BT 节点必须稳定返回 SUCCESS / FAILURE / RUNNING。恢复条件应绑定可观察状态，而不是隐式超时：

ProgressChecker 失败 → 卡住，走 clear/spin。
GoalChecker 超时 → 目标不可达，向上层 FAILURE。
定位协方差超阈 → 应进 relocalization 分支，而不是继续 FollowPath。

RecoveryNode 的 number_of_retries 是有界重试的硬约束。无界重试会在 clear/spin/backup 间振荡——现场最难看的行为：既像有智能，又像没主见。

4. 自定义节点与版本对齐

自定义 BT 节点通过 nav2_behavior_tree 插件注册，XML 里 <MyCustomNode/> 必须对应已编译进 bt_navigator 的 .so。常见幽灵行为：仿真用新 XML、真机旧插件——节点找不到或语义变了，日志却无明确报错。BT XML 与插件 .so 必须同版本发布；改 XML 与改控制器 yaml 分开提交。

给关键 transition 打日志或开 Groot 黑盒：

bash

ros2 param set /bt_navigator enable_groot_monitoring true

无日志的 BT 等于不可调试的状态机——复盘时无法回答「为什么转圈」。

5. 与 progress checker、碰撞监测的分工

组件	检测什么	BT 响应
ProgressChecker	窗口内无有效位移	触发 recovery
CollisionMonitor	进入危险区	减速/停/上报
GoalChecker	到点或超时	SUCCESS / FAILURE

Progress 失败应是 BT 可区分的失败原因，从而选择 clear、等待或中止——不是与「目标不可达」走同一恢复链。区分「卡住」与「不可达」能少做很多无用旋转。

6. 场景袋验证

固定三类场景，改 BT 前后各跑 10 次：

窄门失败：路径存在但跟踪卡死，应在 retry 上限内进入 FAILURE 或有效 recovery。
定位丢失：inject map→odom 跳变，应走 relocalization 而非无限 FollowPath。
目标被挡：家具堵住目标格，GoalChecker 超时后向上层报告，不是无限 replan。

成功率、恢复次数、总耗时三列输出——禁止只看「能不能到」。

7. 验收

恢复动作有 number_of_retries 与总时限；超限 FAILURE 给上层。
关键 transition 有日志或 Groot 记录，可复盘路径。
BT XML 与插件版本绑定发布；改 XML 单独 PR。
场景袋三类全过；振荡次数为 0。

失败策略写进 BT，控制器只管跟路径——决策与运动学分家，调参才有因果。

8. PipelineSequence 与 ReactiveFallback

Nav2 默认导航树用 PipelineSequence 串行：先 ComputePathToPose，再 FollowPath——前一步 SUCCESS 才进下一步。Recovery 外层用 ReactiveFallback：子节点按优先级尝试，GoalUpdated 可打断恢复。理解这两个组合语义，才能改 XML 而不破坏顺序。把 FollowPath 放进 Parallel 与规划并行，会导致未规划就跟踪——常见改坏方式。

RateController hz="1.0" 限制重规划频率，避免每帧 replan 吃满 CPU。Hz 过高：规划抖动；过低：动态障碍反应慢。与 controller_frequency 分开调——BT 重规划频率 ≠ 控制输出频率。

BT 节点通过 nav2_behavior_tree 插件库注册，XML 里引用的节点名必须在 launch 的 plugin_lib_names 列表中。漏注册会 runtime 报错 Node type not recognized——改 XML 前确认插件列表同步更新。

9. 案例：无界 spin 耗尽电池

默认 recovery 链里 Spin 无总时限，窄道卡死后 spin→clear→spin 循环 40 分钟。加 RoundRobin 轮次上限 + progress 失败直出 FAILURE 后，同类场景 2 分钟内上报「不可达」。BT 改两行 XML，比降 max_vel 有效。