Navigate To Pose and Pause Near Goal Obstacle Doesn't Work

[yigitboracagiran]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • Humble
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

1- I've changed the default_nav_to_pose_bt_xml from bt_navigator's parameters and set the value to <path_to>nav_to_pose_and_pause_near_goal_obstacle.xml

nav_to_pose_and_pause_near_goal_obstacle.xml:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <ReactiveSequence name="MonitorAndFollowPath">
          <PathLongerOnApproach path="{path}" prox_len="10.0" length_factor="2.0">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="10.0"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="{selected_controller}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </RecoveryNode>
        </ReactiveSequence>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <RoundRobin name="RecoveryActions">
          <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"/>
          </Sequence>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5.0"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

2- Running ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

3- Running ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True

4- Give a NAV2 Goal to the robot.

5- Put an obstacle in the path.

Expected behavior

Robot should wait for wait_duration parameter inside the CancelingControlAndWait.

Actual behavior

I can't make the robot to wait. When I put an obstacle dynamically in Gazebo, it recalculates the path and it moves to the goal, instead of waiting for wait_duration parameter inside the CancelingControlAndWait.

Additional information

https://github.com/user-attachments/assets/27f89ed9-9ffd-40f1-9685-b78a68705abd

---