Untitled

<!--
  This Behavior Tree replans the global path periodically at 1 Hz through an array of poses continuously
   and it also has recovery actions specific to planning / control as well as general system issues.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence>
      <ProgressCheckerSelector selected_progress_checker="{selected_progress_checker}" default_progress_checker="progress_checker" topic_name="progress_checker_selector"/>
      <GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="general_goal_checker" topic_name="goal_checker_selector"/>
      <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
      <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
      <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}"/>
      <PipelineSequence name="NavigateLoop">
        <RateController hz="2.0">
          <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
        </RateController>
        <Fallback name="FollowOrWait">
          <ReactiveSequence name="SafeToFollow">
            <IsPathValid path="{path}"/>
            <FollowPath path="{path}" controller_id="{selected_controller}"/>
          </ReactiveSequence>
          <Sequence name="WaitThenReplan">
            <!-- <TruncatePath path="{path}"/>  -->
            <Wait wait_duration="4.0"/>
            <RemoveInCollisionGoals input_goals="{goals}" output_goals="{goals}" nb_goals_to_consider = "10" cost_threshold="254.0" use_footprint="true" service_name="/global_costmap/get_cost_global_costmap" />
            <ComputePathThroughPoses goals="{goals}" path="{path}"
                                    planner_id="{selected_planner}"/>
            <AlwaysSuccess/>
          </Sequence>
        </Fallback> 
      </PipelineSequence>
    </Sequence>
  </BehaviorTree>
</root>