ROS2与Navigation2入门教程-导航至某个位姿（Navigate To Pose）行为树

说明：

• 介绍导航至某个位姿（Navigate To Pose）行为树

导航至某个位姿（Navigate To Pose）行为树

• 此行为树是在行为树XMLs之上实现的一个明显更加成熟版本的行为树。

• 该行为树会让机器人在自由空间中从起点导航到单点目标。

• 该行为树同时包含在特定子环境中使用自定义恢复器和用于系统级故障的全局恢复器子树。

• 该行为树还为用户提供了在返回失败状态之前多次重试任务的机会。

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

• 在此行为树中，在向调用者返回任务失败的消息之前，会尝试重试整个导航任务6次。这样可以使导航系统有充足的机会尝试从故障条件中恢复或等待瞬态问题过去，例如人群拥挤或传感器临时故障。

• 在名义执行中，这样会每秒重新规划路径并将该路径传递给控制器​​，类似于行为树XMLs中的行为树。然而，此时如果规划器失败，该行为树会在其子树中触发上下文感知恢复器，用于清除全局成本地图。可以在此处添加其他恢复器以进行其他特定于上下文的恢复，例如尝试其他算法。

• 同样地，控制器也有类似的逻辑。如果控制器失败，也会尝试对影响控制器的本地成本地图进行成本地图清除。值得注意的是在反应回退中的GoalUpdated节点。这样可以通过抢占将新目标传递给导航系统时退出恢复器条件。这样可以确保导航系统会在发布新目标时立即做出响应，即使最后的目标处于尝试恢复过程也是如此。

• 如果这些上下文恢复器失败，则此行为树会进入恢复子树。此子树保留用于系统级故障，以帮助解决像机器人卡住或处于坏点等这样的问题。这个子树也有BT节点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">
            <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_na    me="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>

参考：

• https://navigation.ros.org/behavior_trees/trees/nav_to_pose_recovery.html