<!--
  Ackermann NavigateThroughPoses behavior tree.

  Planning failure is retried once after the global costmap has been cleared
  and given one update period. FollowPath is not replanned periodically while
  it is RUNNING. A controller failure gets one local-costmap retry, then one
  short backup followed by one final through-poses plan and follow attempt.
  Any final failure aborts the navigation action.
-->
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="NavigateThroughPosesOnce">
      <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
        <ReactiveSequence>
          <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
          <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
        </ReactiveSequence>
        <Sequence name="ClearGlobalAndWait">
          <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          <Wait wait_duration="1"/>
        </Sequence>
      </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"/>
          <ReactiveSequence>
            <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
            <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
          </ReactiveSequence>
          <FollowPath path="{path}" controller_id="FollowPath"/>
        </Sequence>
      </Fallback>
    </Sequence>
  </BehaviorTree>
</root>
