Navigate Through Poses

此行为树实现了从起点开始，经过许多中间硬姿势约束，到达自由空间中的最终目标的导航行为。它既包含在特定子上下文中恢复的自定义行为的使用，也包含在系统级故障的全局恢复子树中。它还为用户提供了在返回失败状态之前多次重试任务的机会。

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

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

在正常执行中，这将每 3 秒重新规划一次路径并将该路径传递给控制器，类似于：ref:behavior_trees 中的行为树。不过，规划器现在是 ComputePathThroughPoses，采用向量 goals，而不是要规划的单个姿势 goals。 RemovePassedGoals 节点用于剔除机器人在其路径上经过的 goals。在这种情况下，当机器人在目标的 0.5 以内并且是列表中的下一个目标时，它被设置为从姿势中删除姿势。这样做的目的是，可以在机器人经过一些中间姿势后计算重新规划，而不是继续尝试在将来通过它们进行重新规划。这次，如果规划器失败，它将触发其子树中的上下文感知恢复，清除全局代价地图。可以在此处添加其他恢复，以进行其他上下文特定恢复，例如尝试另一种算法。

同样，控制器也有类似的逻辑。如果失败，它还会尝试清除影响控制器的本地代价地图。值得注意的是反应式回退中的“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="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
            </ReactiveSequence>
            <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_error_code}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>