Skip to content
Anima Mundi Anima Mundi

目标附近障碍物前暂停的行为树

注意：阅读本文前，建议先阅读行为树文档，其中介绍了本树使用的各类行为节点，例如 ReactiveSequence、SequenceWithMemory 和 RetryUntilSuccessful。

该行为树是 导航到目标位姿行为树 的一个软扩展（soft extension）。在原有功能基础上，它增加了一种处理目标附近临时障碍物（如叉车、人员等）的机制：机器人暂停导航并等待一段指定时间，观察障碍物是否移走。若等待期间障碍物已经移开，机器人沿较短的路径继续前往目标；若障碍物始终未移走或等待超时，则沿较长的绕行路径到达目标。这样做可以有效解决目标附近出现临时障碍物时路径变长、任务循环时间增加的问题。

该行为树如下图所示。从图中可以看到，导航子树（Navigation Subtree）中多了一个名为 MonitorAndFollowPath 的分支。该分支用于实现各种监控行为。在本行为树中，监控分支仅使用 PathLongerOnApproach 节点，用于检测全局规划器在接近目标附近时是否规划出一条明显更长的路径。如果没有明显更长的路径，监控节点将进入 FollowPath 恢复节点，由后者生成相应的控制命令。

patience_and_recovery

当出现明显更长的路径时，PathLongerOnApproach 节点的子节点就会被触发。该子节点是一个 RetryUntilSuccessful 装饰器节点，其下挂一个 SequenceWithMemory 节点。SequenceWithMemory 节点首先触发 CancelControl 节点，取消控制器服务器，使机器人停止导航；随后触发 Wait 节点，让机器人等待一段指定时间。由于 MonitorAndFollowPath 是一个 ReactiveSequence 节点，PathLongerOnApproach 节点必须返回 SUCCESS，FollowPath 节点才能再次被触发。

在下面的 GIF 中可以看到，机器人正在接近目标位置，但在目标附近发现了障碍物，于是全局规划器规划了一条较长的绕行路径。此时 PathLongerOnApproach 被触发并调用其子节点，取消 controller_server 并等待，观察障碍物是否会移走。在下面的场景中，障碍物始终未移走，机器人最终沿较长的路径绕行。

nav2_patience_near_goal_and_go_around

另一种情况是，如果等待期间障碍物已经移走，全局规划器会生成一条较短的路径。此时 PathLongerOnApproach 返回 SUCCESS，FollowPath 继续控制机器人导航。

nav2_patience_near_goal_and_clear_obstacle

此外，如果障碍物在指定的等待时间内始终未移走，机器人也会沿较长的路径前往目标位置。

总之，该行为树既可以作为示例参考，也可以直接用于实际场景，帮助需要优化任务循环时间的应用提升效率。

  <root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
        <PipelineSequence name="NavigateWithReplanning">
          <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">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </RecoveryNode>
          </RateController>
          <ReactiveSequence name="MonitorAndFollowPath">
            <PathLongerOnApproach path="{path}" prox_len="3.0" length_factor="2.0">
              <RetryUntilSuccessful num_attempts="1">
                <SequenceWithMemory name="CancelingControlAndWait">
                  <CancelControl name="ControlCancel"/>
                  <Wait wait_duration="5.0"/>
                </SequenceWithMemory>
              </RetryUntilSuccessful>
            </PathLongerOnApproach>
            <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}"/>
              <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
            </RecoveryNode>
          </ReactiveSequence>
        </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_global_costmap"/>
            </Sequence>
            <Spin spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
          </RoundRobin>
        </ReactiveFallback>
      </RecoveryNode>
    </BehaviorTree>
  </root>

下面的视频展示了该行为树的完整演示：

<iframe width="560" height="315" src="https://www.youtube-nocookie.com/embed/7AkZiH2Cf_I" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>