<?xml version="1.0"?>
<!--
  navigate_to_pose_with_recovery.xml — SaltyBot Nav2 behavior tree
  Format: BehaviorTree.CPP v4 (ROS2 Humble)

  Recovery sequence (outer RecoveryNode, 6 retries):
    1. Clear local + global costmaps
    2. Spin 90°  (1.57 rad)
    3. Wait 5 s
    4. Back up 0.30 m

  Each recovery runs round-robin until navigation succeeds or retries exhausted.
-->
<root BTCPP_format="4">
  <BehaviorTree ID="NavigateToPoseWithRecovery">

    <RecoveryNode number_of_retries="6" name="NavigateRecovery">

      <!-- ── Normal navigation: replan at 1 Hz, follow path ── -->
      <PipelineSequence name="NavigateWithReplanning">

        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathRecovery">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ReactiveFallback name="ComputePathRecoveryFallback">
              <GoalUpdated/>
              <ClearEntireCostmap
                name="ClearGlobalCostmap-Context"
                service_name="global_costmap/clear_entirely_global_costmap"/>
            </ReactiveFallback>
          </RecoveryNode>
        </RateController>

        <RecoveryNode number_of_retries="1" name="FollowPathRecovery">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ReactiveFallback name="FollowPathRecoveryFallback">
            <GoalUpdated/>
            <ClearEntireCostmap
              name="ClearLocalCostmap-Context"
              service_name="local_costmap/clear_entirely_local_costmap"/>
          </ReactiveFallback>
        </RecoveryNode>

      </PipelineSequence>

      <!-- ── Recovery behaviors (round-robin) ── -->
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">

          <!-- 1. Clear both costmaps -->
          <Sequence name="ClearingActions">
            <ClearEntireCostmap
              name="ClearLocalCostmap-Subtask"
              service_name="local_costmap/clear_entirely_local_costmap"/>
            <ClearEntireCostmap
              name="ClearGlobalCostmap-Subtask"
              service_name="global_costmap/clear_entirely_global_costmap"/>
          </Sequence>

          <!-- 2. Spin 90° to detect surroundings -->
          <Spin spin_dist="1.57"/>

          <!-- 3. Wait 5 s (e.g. person clears path) -->
          <Wait wait_duration="5"/>

          <!-- 4. Back up 0.30 m at 0.15 m/s -->
          <BackUp backup_dist="0.30" backup_speed="0.15"/>

        </RoundRobin>
      </ReactiveFallback>

    </RecoveryNode>
  </BehaviorTree>
</root>
