导航至姿势并在目标障碍附近暂停

此行为树是导航至目的地的软扩展。除了导航至目的地的功能外，此行为树还允许机器人通过暂停机器人导航并等待用户指定的时间来检查障碍物是否已清除，从而有效地处理靠近目标的障碍物（例如叉车、人或其他临时障碍物）。如果障碍物在等待时间内移动，机器人将继续沿较短路径到达目标。如果障碍物在等待时间内没有移动或等待时间已到，则机器人将绕行较长的路径到达最终目标位置。最终，对于给定的任务，此行为树有助于解决长周期时间的问题，这是由于靠近目标位置的临时障碍物导致路径较长而导致的。

行为树如下图所示。从图中可以看出，导航子树中有一个称为“MonitorAndFollowPath”的附加分支。创建此分支的目的是为了让用户执行他们的机器人应该表现出的任何类型的监控行为。在这个特定的 BT 中，监控分支专门由 PathLongerOnApproach BT 节点使用，用于检查全局规划器是否已决定在接近用户指定的目标接近度时为机器人规划一条明显更长的路径。如果没有明显更长的路径，监控节点将进入 FollowPath 恢复节点，然后生成必要的控制命令。

一旦有一条明显更长的路径，PathLongerOnApproach 节点的子节点就会勾选。子节点是一个 RetryUntilSuccesfull 装饰器节点，它又有一个 SequenceStar 节点作为其子节点。首先，SequenceStar 节点通过勾选 CancelControl 节点取消控制器服务器。取消控制器服务器会停止机器人的进一步导航。接下来，SequenceStar 节点勾选 Wait 节点，使机器人能够等待用户指定的时间。这里我们需要注意，MonitorAndFollowPath 是一个 ReactiveSequence 节点，因此 PathLongerOnApproach 节点需要返回 SUCCESS，然后才能再次勾选 FollowPath 节点。在下面的 GIF 中，可以看到机器人正在接近目标位置，但它在目标附近发现了一个障碍物，因此全局规划器会规划一条更长的路径。这是 PathLongerOnApproach 勾选并勾选其子节点的点，从而取消 controller_server 并等待查看障碍物是否清除。在下面的场景中，障碍物没有清除，导致机器人走更长的路径。

../../_images/nav2_patience_near_goal_and_go_around.gif

或者，如果障碍物被清除，则全局规划器会生成一条更短的路径。现在，PathLongerOnApproach 返回 SUCCESS，这导致 FollowPath 继续进行机器人导航。

../../_images/nav2_patience_near_goal_and_clear_obstacle.gif

除了上述场景外，我们还需要注意，如果障碍物在给定的用户特定等待时间内没有清除，机器人将采取更长的路径到达目标位置。

总之，这个特定的 BT 既可以作为示例，也可以作为希望优化其流程周期时间的组织特定应用的现成 BT。

<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}"/>
            <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">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="5.0"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
            <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"/>
          <Wait wait_duration="5.0"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

A complete demo of this BT can be seen in the video below: