导航至目的地

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

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

在此行为树中，我们尝试重试整个导航任务 6 次，然后向调用者返回任务失败的消息。这使导航系统有充足的机会尝试从故障情况中恢复或等待瞬态问题过去，例如人群拥挤或传感器暂时故障。

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

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

如果这些上下文恢复失败，此行为树将进入恢复子树。此子树保留用于系统级故障，以帮助解决机器人卡住或处于糟糕位置等问题。此子树还具有``GoalUpdated`` BT节点，它每次迭代都会勾选以确保新目标的响应能力。接下来，恢复子树将进行恢复：代价地图清除操作、旋转、等待和备份。在子树中的每次恢复之后，将重新尝试主导航子树。如果它继续失败，则勾选恢复子树中的下一次恢复。

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

<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}"/>
            <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}"/>
          <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}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_code_id}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>