导航到姿势并在目标障碍物附近暂停

注解

作为先决条件，我们鼓励用户阅读`行为树文档 <https://behaviortree.github.io/BehaviorTree.CPP/>`_，该文档解释了这些树中使用的不同行为节点，如``ReactiveSequence``、SequenceStar``和``RetryUntilSucessfull。

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

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

一旦存在明显较长的路径，子节点 PathLongerOnApproach 将开始执行。子节点是一个 RetryUntilSuccesfull 装饰器节点，其下有一个 SequenceStar 节点作为其子节点。首先，SequenceStar 节点通过执行 CancelControl 节点取消控制器服务器。取消控制器服务器会停止机器人的进一步导航。接下来，SequenceStar 节点会执行 Wait 节点，使机器人等待给定的用户指定时间。需要注意的是，MonitorAndFollowPath 是一个 ReactiveSequence 节点，因此在 FollowPath 节点可以再次执行之前，PathLongerOnApproach 节点需要返回成功（SUCCESS）。

在下面的GIF图中，可以看到机器人正在接近目标位置，但在目标附近发现了障碍物，因此全局规划器规划了一条更长的路径。这是``PathLongerOnApproach``节点开始工作并激活

或者，如果障碍物被清除，则全局规划器会生成较短的路径。现在，PathLongerOnApproach 返回成功（SUCCESS），导致 FollowPath 继续机器人的导航。

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

总而言之，这个特定的行为树既可以作为示例，也可以作为组织特定应用的即用行为树，以优化其流程周期时间。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <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}" planner_id="GridBased"/>
            <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"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="FollowPath"/>
            <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"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

可以在下面的视频中看到此行为树的完整演示：