导航到姿势

该行为树在 Nav2行为树上实现了一个更成熟的版本。它可以在自由空间中从起点导航到单一目标点。它包含在特定子上下文中使用自定义恢复行为以及用于系统级故障的全局恢复子树。它还为用户提供了在返回失败状态之前多次重试任务的机会。

ComputePathToPose 和 FollowPath 的BT节点也指定了它们要使用的算法。按照惯例，我们按算法的风格命名它们（例如，不是 DWB 而是 FollowPath），这样行为树或应用程序开发人员就不需要关注技术细节。他们只想使用一个路径跟踪控制器。

在这个行为树中，我们尝试在返回任务失败之前重试整个导航任务 6 次。这使得导航系统有充分的机会尝试从故障条件中恢复或等待瞬时问题解决，例如人员拥挤或临时传感器故障。

在正常执行中，它将每秒重新规划路径并将该路径传递给控制器，类似于 Nav2行为树中的行为树。然而，这次如果规划器失败，它将在其子树中触发具有上下文意识的恢复行为，并清除全局代价地图。可以在此处添加其他上下文特定的恢复行为，例如尝试另一种算法。

同样，控制器也有类似的逻辑。如果控制器失败，它也会尝试清除本地代价地图，从而影响控制器的行为。值得注意的是反应式回退中的``GoalUpdated``节点。这使得当通过抢占向导航系统传递新目标时，我们能够退出恢复条件。这确保在发出新目标时，导航系统将立即做出非常快速的响应，即使最后一个目标处于恢复尝试中的状态。

如果这些上下文恢复失败，则行为树进入恢复子树。这个子树专门用于解决系统级故障，例如机器人被卡住或处于糟糕位置的问题。该子树还有一个``GoalUpdated``行为树节点，在每次迭代中进行刻度以确保对新目标的响应性。接下来，恢复子树将进行以下恢复操作：清除代价地图、旋转、等待和后退。在子树中的每个恢复操作之后，将重新尝试主导航子树。如果继续失败，则刻度恢复子树中的下一个恢复操作。

虽然此行为树没有使用，但是``PlannerSelector``、ControllerSelector``和``GoalCheckerSelector``行为树节点也可能很有帮助。与硬编码要使用的算法（``GridBased``和``FollowPath）不同，这些行为树节点将允许用户通过ROS主题动态更改导航系统中使用的算法。相反，可能更明智的做法是使用条件节点在最有用和独特的情况下创建具有指定算法的不同子树上下文。然而，选择节点可以是一种有用的方式，通过外部应用程序而不是通过内部行为树控制流逻辑来更改算法。最好通过行为树方法来实现更改，但我们理解许多专业用户有外部应用程序以动态更改其导航器的设置。

<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"/>
            <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
              <GoalUpdated/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </ReactiveFallback>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ReactiveFallback name="FollowPathRecoveryFallback">
            <GoalUpdated/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </ReactiveFallback>
        </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_global_costmap"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
          <BackUp backup_dist="0.15" backup_speed="0.025"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>