<!--
  Ackermann NavigateToPose behavior tree.

  Compute the path once and keep the same path while FollowPath is RUNNING.
  A new global plan is requested only after FollowPath returns FAILURE and the
  outer RecoveryNode starts another navigation attempt. This avoids replacing
  a valid path every second while MPPI is still evaluating it.

  Recovery actions intentionally do not include Spin or BackUp because this
  robot uses an Ackermann drive model.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <Sequence name="NavigateOnce">
        <RecoveryNode number_of_retries="1" name="ComputePathToPose">
          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
        </RecoveryNode>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </Sequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <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"/>
          <Wait wait_duration="1"/>
        </Sequence>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>
