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

该行为树是行为树中基础行为树的进阶版本，负责从起始点导航到自由空间中的单个目标点。它既包含针对特定场景的局部恢复行为，也有处理系统级故障的全局恢复子树，并在最终返回失败前提供多次重试机会。

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

该行为树在返回失败前，会尝试重新执行整个导航任务最多 6 次，为导航系统提供充分的恢复机会，或者等待临时性问题（如人群拥挤、传感器短暂故障）自行消除。

正常运行时，若距离目标不够近，行为树会以每秒一次的频率重新规划路径并将其传递给控制器，这一点与行为树中的基础版本类似。不同之处在于：当规划器失败时，会在子树中触发场景化恢复行为，清除全局代价地图。此处还可添加其他恢复行为来应对更多场景，例如尝试换用另一种算法。

控制器也有类似的逻辑：若控制器执行失败，会尝试清除影响其运行的局部代价地图。这里需要关注 ReactiveFallback（反应式回退）节点中的 GoalUpdated 节点。当通过抢占（preemption）机制向导航系统传递新目标时，该节点能够中断当前恢复流程，确保即使上一个目标正处于恢复尝试中，导航系统也能在新目标发出后立即响应。

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

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

  <root BTCPP_format="4" main_tree_to_execute="NavigateToPoseWReplanningAndRecovery">
    <BehaviorTree ID="NavigateToPoseWReplanningAndRecovery">
      <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="1.0">
            <RecoveryNode number_of_retries="1" name="ComputePathToPose">
              <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>
                <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
              </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}" 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 spin_dist="1.57" error_code_id="{spin_error_code}" error_msg="{spin_error_msg}"/>
              <Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
              <BackUp 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>