带恢复的穿越目标位姿导航行为树

该行为树实现了一种导航行为：从起始点出发，依次穿过多个中间硬位姿约束（hard pose constraints），最终到达自由空间中的目标点。它在特定场景中使用了自定义恢复行为，同时也包含用于系统级故障的全局恢复子树。此外，在最终返回失败之前，它会多次重试整个导航任务。

ComputePathThroughPoses 和 FollowPath 行为树节点也都指定了要使用的算法。按照惯例，这些节点以功能风格而非具体算法命名（例如使用 FollowPath 而非 DWB），这样行为树或应用开发者无需关注底层技术细节，只需指定一个路径跟随控制器即可。

该行为树在最终返回失败之前，会尝试重试整个导航任务 6 次。这为导航系统提供了充足的机会来从故障中恢复，或等待暂时性问题（例如人群拥挤或传感器临时故障）自行消退。

正常运行时，如果距离目标不够近，它会每 3 秒重新规划一次路径并将路径传递给控制器，这一点与行为树中的行为树类似。不同之处在于，此处规划器为 ComputePathThroughPoses，它接收一个目标列表 goals 而非单个位姿 goal 来进行规划。RemovePassedGoals 节点用于剔除机器人已经经过的航点。在本例中，当机器人距离某个航点 0.5 以内、且该航点是列表中的下一个目标时，就会将其从列表中移除。此外，该节点还会在 waypoint_statuses 中记录每个航点的状态（例如 PENDING、COMPLETED、SKIPPED 或 FAILED）。这样设计使得机器人在经过部分中间航点后可以重新规划路径，而无需继续规划已通过的航点。

如果规划器失败，会在其子树中触发场景化恢复行为，清除全局代价地图。也可以在此处添加其他局部恢复行为，例如尝试换用另一种算法。

控制器也有类似的逻辑：如果控制器失败，会尝试清除局部代价地图。这里需要关注 ReactiveFallback（反应式回退）节点中的 GoalUpdated 节点。当导航系统通过抢占（preemption）收到新目标时，该节点可以让我们跳出恢复流程。这样即使上一个目标正处于恢复尝试中，导航系统也能在收到新目标后立即响应。

如果这些场景化恢复仍然失败，行为树将进入恢复子树。该子树专门用于处理系统级故障，帮助解决诸如机器人被困或处于不利位置等问题。子树中也包含 GoalUpdated 行为树节点，每次迭代都会触发，以确保对新目标快速响应。随后，恢复子树将依次执行代价地图清除、原地旋转、等待和后退等恢复动作。每次恢复动作执行完毕后，都会重新尝试主导航子树。如果仍然失败，则触发恢复子树中的下一个恢复动作。

虽然该行为树未使用这些节点，但 PlannerSelector、ControllerSelector、GoalCheckerSelector、ProgressCheckerSelector 和 PathHandlerSelector 等行为树节点也十分有用。这些节点无需硬编码所用算法（GridBased 和 FollowPath），而是允许通过 ROS 话题动态切换导航系统中的算法。另一种更推荐的做法是，在特定场景下使用条件节点指定算法，从而创建不同的子树分支。不过，选择器节点提供了一种从外部应用（而非行为树内部控制流逻辑）切换算法的便捷方式。通过行为树内部控制流实现切换是更好的方案，但许多专业用户拥有自己的外部应用来动态调整导航器设置，选择器节点正好满足这一需求。

  <root BTCPP_format="4" main_tree_to_execute="NavigateThroughPosesWReplanningAndRecovery">
    <BehaviorTree ID="NavigateThroughPosesWReplanningAndRecovery">
      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
        <PipelineSequence name="NavigateWithReplanning">
          <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
          <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
          <PathHandlerSelector selected_path_handler="{selected_path_handler}" default_path_handler="PathHandler" topic_name="path_handler_selector"/>
          <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">
              <Fallback name="FallbackComputePathToPose">
                <ReactiveSequence name="CheckIfNewPathNeeded">
                  <Inverter>
                    <GlobalUpdatedGoal/>
                  </Inverter>
                  <IsGoalNearby path="{path}" proximity_threshold="4.0" max_robot_pose_search_dist="1.5"/>
                  <TruncatePathLocal input_path="{path}" output_path="{remaining_path}" distance_forward="-1" distance_backward="0.0" />
                  <ValidatePath path="{remaining_path}"/>
                </ReactiveSequence>
                <ReactiveSequence>
                  <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7" input_waypoint_statuses="{waypoint_statuses}" output_waypoint_statuses="{waypoint_statuses}"/>
                  <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                </ReactiveSequence>
              </Fallback>
              <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}" error_msg="{follow_path_error_msg}" goal_checker_id="{selected_goal_checker}" progress_checker_id="{selected_progress_checker}" path_handler_id="{selected_path_handler}" tracking_feedback="{tracking_feedback}"/>
            <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 name="SpinRecovery" spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
              <Wait name="WaitRecovery" wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
              <BackUp name="BackUpRecovery" backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
            </RoundRobin>
          </ReactiveFallback>
        </Sequence>
      </RecoveryNode>
    </BehaviorTree>
  </root>