行为树详解

概述
前提条件
带重新规划与恢复的导航到目标位姿
导航子树
恢复子树

概述

本文档是 Nav2 中使用的主要行为树（BT）的参考指南。

nav2_bt_navigator/behavior_trees 目录中提供了许多示例行为树，但有时需要根据机器人的实际应用重新配置。本文档将详细介绍当前的主要默认 BT navigate_to_pose_w_replanning_and_recovery.xml。

前提条件

在继续阅读本节之前，请先熟悉行为树的概念
- 阅读导航概念中的简要说明
- 阅读 BehaviorTree.CPP V4 网站上的通用教程和指南（不限于 Nav2），尤其是 “Basic Concepts” 部分，其中介绍了本指南所用到的各类基础通用节点。
熟悉自定义的 Nav2 专用 BT 节点

带重新规划与恢复的导航到目标位姿

本节详细描述 Nav2 当前使用的主要默认 BT navigate_to_pose_w_replanning_and_recovery.xml。该行为树以 1 Hz 的频率定期重新规划全局路径，并包含恢复行为。

overall_bt

BT 主要用 XML 定义，上面显示的树对应的 XML 如下。

<root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
    <BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
        <RecoveryNode number_of_retries="6" name="NavigateRecovery">
            <PipelineSequence name="NavigateWithReplanning">
                <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
                <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
                <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
                <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
                <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
                <RateController hz="1.0">
                    <RecoveryNode number_of_retries="1" name="ComputePathToPose">
                        <Fallback name="FallbackComputePathToPose">
                            <ReactiveSequence name="CheckIfNewPathNeeded">
                                <Inverter>
                                    <GlobalUpdatedGoal/>
                                </Inverter>
                                <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                                <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                                <ValidatePath path="{remaining_path}"/>
                            </ReactiveSequence>
                            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                        </Fallback>
                        <Sequence>
                            <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
                            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
                        </Sequence>
                    </RecoveryNode>
                </RateController>
                <RecoveryNode number_of_retries="1" name="FollowPath">
                    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
                    <Sequence>
                        <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
                        <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
                    </Sequence>
                </RecoveryNode>
            </PipelineSequence>
            <Sequence>
                <Fallback>
                    <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
                    <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
                </Fallback>
                <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_global_costmap"/>
                        </Sequence>
                        <Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
                        <Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
                        <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
                    </RoundRobin>
                </ReactiveFallback>
            </Sequence>
        </RecoveryNode>
    </BehaviorTree>
</root>

这棵树可能看起来有些复杂，但可以分解为两个较小的子树来逐一分析。这两个子树是最顶层 RecoveryNode 的子节点。下文中，NavigateWithReplanning 子树简称为 Navigation 子树，RecoveryFallback 子树简称为 Recovery 子树。简化后的结构如下：

overall_bt_w_breakdown

Navigation 子树主要涉及实际的导航行为：

选择规划器 / 控制器 / 目标检查器 / 路径处理器 / 进度检查器插件
计算路径
跟随路径
针对上述每个主要导航行为的场景化恢复行为

Recovery 子树负责处理系统级故障或难以在导航过程中解决的问题。

正常运行时，整个 BT 大部分时间会停留在 Navigation 子树中。如果 Navigation 子树中的两个主要行为（路径计算或路径跟随）中任何一个失败，会先尝试场景化恢复。

如果场景化恢复仍然不够，Navigation 子树将返回 FAILURE，系统进入 Recovery 子树，尝试清除系统级导航故障。

这一过程会持续进行，直到超过父级 RecoveryNode 的 number_of_retries（默认为 6）。

<RecoveryNode number_of_retries="6" name="NavigateRecovery">

导航子树

了解了 Navigation 子树和 Recovery 子树之间的控制流后，下面重点分析 Navigation 子树。

navigation_subtree

该子树的 XML 如下：

<PipelineSequence name="NavigateWithReplanning">
    <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
    <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
    <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
    <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
    <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
    <RateController hz="1.0">
      <RecoveryNode number_of_retries="1" name="ComputePathToPose">
        <Fallback name="FallbackComputePathToPose">
          <ReactiveSequence name="CheckIfNewPathNeeded">
            <Inverter>
              <GlobalUpdatedGoal/>
            </Inverter>
            <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
            <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
            <ValidatePath path="{remaining_path}"/>
          </ReactiveSequence>
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
        </Fallback>
        <Sequence>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
        </Sequence>
      </RecoveryNode>
    </RateController>
    <RecoveryNode number_of_retries="1" name="FollowPath">
      <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
      <Sequence>
        <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
        <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
      </Sequence>
    </RecoveryNode>
</PipelineSequence>

该子树有两个主要动作：ComputePathToPose 和 FollowPath。如果其中任何一个失败，会尝试在场景化层面清除故障。这棵树的精髓可以用一个父节点和两个子节点来概括，如下所示：

navigation_subtree_bare

父节点 PipelineSequence 确保 ComputePathToPose 会被 tick，成功后 FollowPath 也会被 tick。当 FollowPath 子树被 tick 时，ComputePathToPose 子树同样会被 tick，从而使机器人在移动过程中持续重新计算路径。

ComputePathToPose 和 FollowPath 都遵循相同的整体结构：

执行动作
如果动作失败，尝试判断能否在场景化层面恢复

下面是 ComputePathToPose 子树：

contextual_recoveries

父节点 RecoveryNode 控制动作与场景化恢复子树之间的控制流。 ComputePathToPose 和 FollowPath 的场景化恢复都包括检查恢复是否有助于清除错误代码，以及清除相关的代价地图。

如果在进入系统级恢复之前，希望容忍更多次的场景化恢复尝试，可以修改父级 RecoveryNode 控制节点中的 number_of_retries 参数。

ComputePathToPose 和 FollowPath 的 BT 子树之间的区别概述如下：

子树中的动作节点：
- ComputePathToPose 子树以 ComputePathToPose 动作（规划器）为核心。
- FollowPath 子树以 FollowPath 动作（控制器）为核心。
条件流程控制（Fallback）的使用：
- ComputePathToPose 子树包含处理机器人接近目标时行为的逻辑。在小半径（例如 1.0 m 以内）范围内重新规划，可能因状态估计漂移或路径跟踪误差而产生不利影响，常常导致不必要的「循环」行为。为避免这种情况，该子树使用带有 IsGoalNearby 节点的 ReactiveSequence。如果机器人位于指定的接近阈值内，并且当前路径仍然有效（即没有新的障碍物），子树将跳过重新规划请求，使机器人能够利用当前路径平滑地过渡到最终接近阶段，而无需不必要的重新规划。
- 相比之下，FollowPath 子树通常不使用这种条件门控。一旦路径可用，控制器会被直接调用来输出速度命令。
装饰 ComputePathToPose 子树的 RateController RateController 装饰 ComputePathToPose 子树，使规划保持指定的频率。该 BT 的默认频率为 1 Hz。这样做是为了防止 BT 以树更新速率（100 Hz）向规划服务器发送过多无用请求。可根据应用场景和计算路径的计算成本，适当调高或调低此频率。除了 RateController，还可以使用其他装饰器，例如在合适场景下使用 SpeedController 或 DistanceController。
场景化恢复中清除的代价地图：
- ComputePathToPose 子树清除全局代价地图。全局代价地图是规划器场景中的相关代价地图。
- FollowPath 子树清除局部代价地图。局部代价地图是控制器场景中的相关代价地图。

该子树还使用了 PlannerSelector、ControllerSelector、GoalCheckerSelector、ProgressCheckerSelector 和 PathHandlerSelector 节点，为需要实时调整导航行为的应用提供了灵活性。

恢复子树

Recovery 子树是 Nav2 默认 navigate_to_pose_w_replanning_and_recovery.xml 树的另一大组成部分。简而言之，当 Navigation 子树返回 FAILURE 时（即其中的场景化恢复不足以解决问题时），该子树被触发，在系统级别执行恢复。

recovery_subtree

XML 片段：

<Sequence>
    <Fallback>
        <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
        <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
    </Fallback>
    <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_global_costmap"/>
            </Sequence>
            <Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
            <Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
        </RoundRobin>
    </ReactiveFallback>
</Sequence>

在顶层，一个 Sequence 确保以下步骤按顺序执行：

一个 Fallback 节点首先检查规划器或控制器恢复是否有助于解决问题。如果其中任何一个返回 SUCCESS，则该 Fallback 节点成功，Sequence 继续执行下一步。
一个 ReactiveFallback 控制其余系统级恢复之间的流程，并异步检查是否收到了新的目标。

如果在任何时候目标被更新，该子树将暂停所有子节点并返回 SUCCESS，从而对新目标做出快速反应，并抢占当前正在执行的恢复。这与 Navigation 子树中的场景化恢复部分类似，是处理「除非某个条件发生，否则执行动作 A」这类情景的常见 BT 模式。

这些条件节点功能强大，通常与 ReactiveFallback 配对使用。例如，可以很容易地将整棵 navigate_to_pose_w_replanning_and_recovery 树包裹在一个带有 isBatteryLow 条件的 ReactiveFallback 中——这意味着该树会正常执行，除非电池电量变低（然后转入另一个用于对接充电的子树）。

如果目标从未被更新，行为树将继续执行 RoundRobin 节点。以下是 BT 中默认的四个系统级恢复：

清除两个代价地图（局部和全局）的序列
Spin 动作
Wait 动作
BackUp 动作

一旦父级 RoundRobin 的四个子节点中任何一个返回 SUCCESS，机器人将尝试在 Navigation 子树中重新导航。如果重新导航不成功，RoundRobin 的下一个子节点将被 tick。

例如，假设机器人被卡住，Navigation 子树返回 FAILURE：（在此示例中，假设目标从未被更新。）

Recovery 子树中的代价地图清除序列被尝试，并返回 SUCCESS。机器人重新进入 Navigation 子树。
假设清除两个代价地图仍然不够，Navigation 子树再次返回 FAILURE。机器人再次进入 Recovery 子树。
在 Recovery 子树中，Spin 动作将被 tick。如果它返回 SUCCESS，机器人将返回主 Navigation 子树；但这里假设 Spin 动作返回 FAILURE，则树将保持在 Recovery 子树中。
假设下一个动作 Wait 返回 SUCCESS，机器人进入 Navigation 子树。
假设 Navigation 子树返回 FAILURE（清除代价地图、尝试旋转和等待仍然不足以恢复系统）。机器人进入 Recovery 子树并尝试 BackUp 动作。假设机器人成功完成 BackUp 动作，该节点返回 SUCCESS，机器人再次进入 Navigation 子树。
在此假设场景中，BackUp 动作使机器人能够在 Navigation 子树中成功导航，最终到达目标。此时整个 BT 返回 SUCCESS。

如果 BackUp 动作仍不足以让机器人脱离卡住状态，上述逻辑将持续循环，直到超过 Navigation 子树和 Recovery 子树的父级 RecoveryNode 中的 number_of_retries，或者 Recovery 子树中的所有系统级恢复都返回 FAILURE（这种情况不太可能发生，通常意味着存在其他系统故障）。