<!--
  Ackermann NavigateToPose behavior tree.

  Compute the path once and keep the same path while FollowPath is RUNNING.
  If following fails, clear the local costmap and retry the same path once.
  If that also fails, back up 0.04 m, compute one final path, and follow it
  once. Any failure in the final branch ends the navigation action.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="NavigateWithSingleBackupReplan">
      <RecoveryNode number_of_retries="1" name="ComputeInitialPath">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
        <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
      </RecoveryNode>

      <Fallback name="FollowOrSingleBackupReplan">
        <RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>

        <Sequence name="BackupReplanAndFinalFollow">
          <BackUp backup_dist="0.04" backup_speed="0.20"/>
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
          <FollowPath path="{path}" controller_id="FollowPath"/>
        </Sequence>
      </Fallback>
    </Sequence>
  </BehaviorTree>
</root>
