Navigate To Pose and Replan Only if Path Invalid

这个行为树实现了:ref:behavior_trees 上的行为树的一个明显更成熟的版本。它在自由空间中从一个起点导航到一个单点目标。它既包含在特定子上下文中使用自定义恢复，也包含用于系统级故障的全局恢复子树。它还为用户提供了在返回失败状态之前多次重试任务的机会。 [校准@混沌无形]

这个 ComputePathToPose 和 FollowPath BT节点都指定了它们要使用的算法。按照惯例，我们用它们的算法风格来命名它们 (例如，不是 DWB ，而是 FollowPath )，这样行为树或应用程序开发人员就不必担心技术细节。他们只想使用路径跟随控制器。 [校准@混沌无形]

在此行为树中，我们尝试重试整个导航任务6次，然后将任务失败状态返回给调用方。这使得导航系统有足够的机会尝试从故障情况中恢复或等待瞬态问题通过，例如来自人员的拥挤或临时传感器故障。 [校准@混沌无形]

In nominal execution, this will replan only if the previous path is invalid and pass a new path onto the controller. However, this time, if the planner fails, it will trigger contextually aware recoveries in its subtree, clearing the global costmap. Additional recoveries can be added here for additional context-specific recoveries, such as trying another algorithm.

同样，控制器具有相似的逻辑。如果失败，它还会尝试清除影响控制器的局部成本地图。值得注意的是反应性回退中的 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">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <Fallback>
              <ReactiveSequence>
                <Inverter>
                  <GlobalUpdatedGoal/>
                </Inverter>
                <IsPathValid path="{path}"/>
              </ReactiveSequence>
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            </Fallback>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <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>
      </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>