导航到姿态

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

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

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

在正常执行中，这将每隔3秒重新规划路径，并将该路径传递给控制器，类似于 Nav2行为树 中的行为树。但是，这次的规划器是``ComputePathThroughPoses``，它接受一个向量``goals``而不是单个姿态``goal``来进行规划。RemovePassedGoals``节点用于删除机器人在路径上已经经过的``goals。在这种情况下，它被设置为在机器人距离目标``0.5``并且它是列表中的下一个目标时，从姿态中移除一个。这样实现后，重新规划可以在机器人经过一些中间姿态后进行计算，而不会在将来继续尝试通过它们进行重新规划。这次，如果规划器失败，它将触发上下文感知的子树中的恢复操作，清除全局代价地图。可以在这里添加其他特定上下文的恢复操作，例如尝试另一个算法。

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

如果这些上下文恢复操作失败，这个行为树将进入恢复子树。这个子树用于处理系统级故障，以帮助解决机器人卡住或处于糟糕位置的问题。这个子树还有``GoalUpdated``BT节点，每次迭代都会被触发，以确保对新目标

虽然此行为树没有使用，但是``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="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.5"/>
              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
            </ReactiveSequence>
            <ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
              <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>