持续重规划与路径失效处理行为树

该行为树是行为树所介绍版本的进阶实现，用于从起点导航到自由空间中的某个目标点。它包含针对特定场景的局部恢复行为，以及用于系统级故障的全局恢复子树，并在最终返回失败前提供多次重试机会。

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

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

正常运行时，重规划可由以下情况触发：之前的路径失效、收到新目标，或 10 秒内未生成新路径。如果规划器或控制器失败，会在各自的子树中触发场景化恢复行为。目前，规划器失败时会清除全局代价地图，控制器失败时则清除局部代价地图，也可以根据需要向这些子树添加更多恢复行为。

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

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

  <root BTCPP_format="4" main_tree_to_execute="NavToPoseWithConsistentReplanningAndIfPathBecomesInvalid">
    <BehaviorTree ID="NavToPoseWithConsistentReplanningAndIfPathBecomesInvalid">
      <RecoveryNode number_of_retries="6" name="NavigateRecovery">
        <PipelineSequence name="NavigateWithReplanning">
          <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="2.0">
            <RecoveryNode number_of_retries="1" name="ComputePathToPose">
              <Fallback>
                <ReactiveSequence>
                  <Inverter>
                    <PathExpiringTimer seconds="10" path="{path}"/>
                  </Inverter>
                  <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>
              <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="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}" tracking_feedback="{tracking_feedback}"/>
            <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 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>
      </RecoveryNode>
    </BehaviorTree>
  </root>