[Behavior Tree] Robot cancels control when in goal x_y tolerance

[chanhhoang99]

Bug report

Required Info:

• Operating System:
• Ubuntu 24.04.1 LTS
• ROS2 Version:
• Rolling
• Version or commit hash:
  • 771eca4
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

1. Use navigate_to_pose_w_replanning_goal_patience_and_recovery.xml.
2. Navigate to pose.

Expected behavior

Robot end up at desire goal without any wait and canceling action.

Actual behavior

1. Robot is able to reach its x_y goal tolerance
2. Robot is rotating to goal heading
3. Wait node ticks.
4. Cancel control ticks.
5. Robot continue to rotate to goal heading after Wait node finishes

Additional information

From above actual behavior, I suspect this relates to PathLongerOnApproach.
I tried reverting commit de4fd78, issue still persists
Please see video. Screencast from 2024-09-18 11-33-48.webm

Below is my configuration:

1. nav2_params.yaml:

amcl:
  ros__parameters:
    set_initial_pose: true
    initial_pose: 
      x: 0.0
      y: 0.0
      yaw: 0.0
    #use_sim_time: False
    alpha1: 0.05
    alpha2: 0.01
    alpha3: 0.04
    alpha4: 0.005
    alpha5: 0.01
    base_frame_id: "base_link"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 10.0
    laser_max_range: 100.0
    laser_min_range: 0.3
    laser_model_type: "likelihood_field"
    max_beams: 1000
    max_particles: 2000
    min_particles: 1000
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::DifferentialMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.15
    update_min_d: 0.25
    z_hit: 0.9
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    # navigators: ["navigate_to_pose", "navigate_through_poses"]
    # navigate_to_pose:
    #   plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    # navigate_through_poses:
    #   plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    default_nav_to_pose_bt_xml: "/home/hoc3hc/nav2_ws/src/navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml"
    #default_nav_through_poses_bt_xml: "/home/hoc3hc/nav2_ws/src/navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml"
    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
    # Built-in plugins are added automatically
    # plugin_lib_names: []

    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

controller_server:
  ros__parameters:
    controller_frequency: 10.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.2
      yaw_goal_tolerance: 0.05
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 30
      model_dt: 0.1
      batch_size: 2000
      ax_max: 0.5
      ax_min: -0.5
      ay_max: 0.0
      az_max: 0.8
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: 0.0
      vy_max: 0.0
      wz_max: 0.8
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.2
      critics: [
        "ConstraintCritic", "CostCritic", "GoalCritic",
        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
        "PathAngleCritic", "PreferForwardCritic"]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 20.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      #CostCritic:
        # enabled: true
        # cost_power: 1
        # cost_weight: 3.81
        # critical_cost: 300.0
        # consider_footprint: true
        # collision_cost: 1000000.0
        # near_goal_distance: 1.0
        # trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      #robot_radius: 0.22
      footprint: "[[-0.32, 0.288], [-0.32, -0.288], [0.32, -0.288], [0.32, 0.288]]"
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.70
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      #robot_radius: 0.22
      footprint: "[[-0.32, 0.288], [-0.32, -0.288], [0.32, -0.288], [0.32, 0.288]]"
      resolution: 0.05
      track_unknown_space: false
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.7
      always_send_full_costmap: True

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
# map_server:
#   ros__parameters:
#     yaml_filename: ""

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: false

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["SmoothPath"]

    SmoothPath:
      plugin: "nav2_constrained_smoother/ConstrainedSmoother"
      reversing_enabled: false       # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned
      path_downsampling_factor: 3   # every n-th node of the path is taken. Useful for speed-up
      path_upsampling_factor: 1     # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling
      keep_start_orientation: true  # whether to prevent the start orientation from being smoothed
      keep_goal_orientation: true   # whether to prevent the gpal orientation from being smoothed
      minimum_turning_radius: 0.40  # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots
      w_curve: 30.0                 # weight to enforce minimum_turning_radius
      w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
      w_smooth: 2000000.0           # weight to maximize smoothness of path
      w_cost: 0.015                 # weight to steer robot away from collision and cost

      # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes)
      w_cost_cusp_multiplier: 3.0   # option to use higher weight during forward/reverse direction change which is often accompanied with dangerous rotations
      cusp_zone_length: 2.5         # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier)

      # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...]
      # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots)
      # cost_check_points: [-0.185, 0.0, 1.0]

      optimizer:
        max_iterations: 70            # max iterations of smoother
        debug_optimizer: false        # print debug info
        gradient_tol: 5000.0
        fn_tol: 0.000000000000001
        param_tol: 0.00000000000000000001

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.6
    min_rotational_vel: 0.4
    rotational_acc_lim: 0.6

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    action_server_result_timeout: 900.0
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.5, 0.0, 2.0]
    min_velocity: [-0.5, 0.0, -2.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

collision_monitor:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.2
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["FootprintApproach"]
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: False
      enabled: True
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True

docking_server:
  ros__parameters:
    controller_frequency: 50.0
    initial_perception_timeout: 5.0
    wait_charge_timeout: 5.0
    dock_approach_timeout: 30.0
    undock_linear_tolerance: 0.05
    undock_angular_tolerance: 0.1
    max_retries: 3
    base_frame: "base_link"
    fixed_frame: "odom"
    dock_backwards: false
    dock_prestaging_tolerance: 0.5

    # Types of docks
    dock_plugins: ['simple_charging_dock']
    simple_charging_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
      docking_threshold: 0.05
      staging_x_offset: -0.7
      use_external_detection_pose: true
      use_battery_status: false # true
      use_stall_detection: false # true

      external_detection_timeout: 1.0
      external_detection_translation_x: -0.18
      external_detection_translation_y: 0.0
      external_detection_rotation_roll: -1.57
      external_detection_rotation_pitch: -1.57
      external_detection_rotation_yaw: 0.0
      filter_coef: 0.1

    # Dock instances
    # The following example illustrates configuring dock instances.
    # docks: ['home_dock']  # Input your docks here
    # home_dock:
    #   type: 'simple_charging_dock'
    #   frame: map
    #   pose: [0.0, 0.0, 0.0]

    controller:
      k_phi: 3.0
      k_delta: 2.0
      v_linear_min: 0.15
      v_linear_max: 0.15

loopback_simulator:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    map_frame_id: "map"
    scan_frame_id: "base_scan"  # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
    update_duration: 0.02

2. navigate_to_pose_w_replanning_goal_patience_and_recovery.xml

<!--
  This BT has all the functionalities of navigate_to_pose_w_replanning_and_recovery.xml,
  with an additional feature to cancel the control closer to the goal proximity and
  make the robot wait for a specific time, to see if the obstacle clears out before
  navigating along a significantly longer path to reach the goal location.
-->
<root BTCPP_format="4" 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}" error_code_id="{compute_path_error_code}"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <ReactiveSequence name="MonitorAndFollowPath">
          <PathLongerOnApproach path="{path}" prox_len="20" length_factor="1.01">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceWithMemory name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="10.0"/>
              </SequenceWithMemory>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
            <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.15"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

[padhupradheep]

Thanks for reporting the issue, let me take a look at it.

[padhupradheep]

It's very strange that I cannot reproduce your issue. I just tried with @neobotix robots and as well both the turtlebots that we have here. Seems to work fine, even with the parameters you have given. I'm certainly sure you would have rebuilt the package, but just for completeness, may I ask did you actually do it?

[chanhhoang99]

Hi @padhupradheep ,
I observe this behavior both on my real robot and turtlebot4 simulation.
The occurrence is not always. But it's not hard to reproduce.
The issue could be reproduced when the goal heading is not in the same direction with the path(In the video attached previously, I tried to give the goal with a different heading at the end.)
And I have built and tested for both commits:

1. 771eca4 (include the latest patch that you have released recently).
2. Revert commit de4fd78.

[padhupradheep]

It could actually be an edge case..

The issue could be reproduced when the goal heading is not in the same direction with the path(In the video attached previously, I tried to give the goal with a different heading at the end.)

I did the same but wasn't able to reproduce it.

I also made a video (sorry for the low quality) with the turtlebot having a different heading at the end:

nav2bug.webm

Is there any other scenario in depot world, where you can show us the issue? would be great if I get some more details. Thanks!

[chanhhoang99]

Okay I have just removed all build artifact (by $rm -r /build /install /log) and rebuild all the nav2 again with latest commit 771eca4.
Issue is reproducible after a few trials. Below is my terminal logs:

[component_container_isolated-9] [INFO] [1726659620.186882210] [bt_navigator]: Begin navigating from current location (15.77, 4.73) to (18.01, -2.39)
[component_container_isolated-9] [INFO] [1726659620.207779929] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-9] [INFO] [1726659621.307983945] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659622.307969344] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659623.307981063] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659624.407961604] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659625.407989273] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659626.407966414] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659627.508026270] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659628.507995824] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659629.507949135] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659630.507971404] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659631.607968496] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659632.607970118] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659633.607967764] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659634.707977162] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659635.708007138] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659636.707965783] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659637.707964830] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659638.807991231] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659639.807978545] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659640.807992836] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659641.807974343] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659642.907969000] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659643.907965740] [controller_server]: Passing new path to controller.
**[component_container_isolated-9] [INFO] [1726659644.887696426] [behavior_server]: Running wait
[component_container_isolated-9] [ERROR] [1726659644.907908415] [bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for follow_path in node halt!
[component_container_isolated-9] [INFO] [1726659644.907951401] [controller_server]: Cancellation was successful. Stopping the robot.
[component_container_isolated-9] [WARN] [1726659644.908178831] [controller_server]: [follow_path] [ActionServer] Client requested to cancel the goal. Cancelling.
[component_container_isolated-9] [INFO] [1726659644.909248916] [controller_server]: Optimizer reset
[component_container_isolated-9] [INFO] [1726659655.087998729] [behavior_server]: wait completed successfully
[component_container_isolated-9] [INFO] [1726659655.099743507] [controller_server]: Received a goal, begin computing control effort.**
[component_container_isolated-9] [INFO] [1726659655.199937299] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659656.199938796] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659657.299927338] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659658.299933281] [controller_server]: Passing new path to controller.
[component_container_isolated-9] [INFO] [1726659658.405593911] [controller_server]: Reached the goal!
[component_container_isolated-9] [INFO] [1726659658.405956410] [controller_server]: Optimizer reset
[component_container_isolated-9] [INFO] [1726659658.429665709] [bt_navigator]: Goal succeeded

From above log, there is a gap before Goal succeeded and the gap is around 10s which is the Wait timeout duration that I set in the xml file.

I also capture the rosbag, the last goal in the bag is where the issue happens ( it takes quite many trials to reproduced :) )
Since it's quite a big bag file, would you mind getting it from https://drive.google.com/file/d/1E789QMxflwApboMJpDJiF_WzPlXVfiJM/view?usp=sharing

[SteveMacenski]

Any thoughts here @padhupradheep ?

[padhupradheep]

Unfortunately it is hard to reproduce the issue in our setup. It's good that the user has given a bag, I'd try to clear it soon.

[chanhhoang99]

Hi @padhupradheep ,
Do you have any findings ? Do you need rosbag file on my real set up. It seems to happen quite often on my real robot.

[padhupradheep]

Unfortunately not yet, I was also away for a conference. Please allow me some more time to get back to this.

[padhupradheep]

@chanhhoang99 like mentioned in #4729 could you please try it without composition and let me know if you can reproduce the issue?

[chanhhoang99]

@padhupradheep , Okay I will check it

[chanhhoang99]

Hi @padhupradheep ,
Seems like it is a bit different when I launch with use_composition:=False.
Logs:

[bt_navigator-16] [INFO 2024-11-23 08:15:01.196] [bt_navigator]: Begin navigating from current location (0.56, 4.81) to (4.36, -6.07)
[controller_server-12] [INFO 2024-11-23 08:15:01.218] [controller_server]: Received a goal, begin computing control effort.
[controller_server-12] [INFO 2024-11-23 08:15:02.318] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:03.318] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:04.318] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:05.418] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:06.418] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:07.418] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:08.418] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:09.518] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:10.518] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:11.518] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:12.518] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:13.618] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:14.618] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:15.618] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:16.618] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:17.718] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:18.718] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:19.718] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:20.718] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:21.818] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:22.818] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:23.818] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:24.918] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:25.918] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:26.918] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:27.918] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:29.186] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:30.186] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:31.186] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:32.187] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:33.118] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:34.118] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:35.118] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:36.118] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:37.218] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:38.218] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:39.218] [controller_server]: Passing new path to controller.
[behavior_server-15] [INFO 2024-11-23 08:15:40.197] [behavior_server]: Running wait
[controller_server-12] [INFO 2024-11-23 08:15:40.218] [controller_server]: Cancellation was successful. Stopping the robot.
[controller_server-12] [WARN 2024-11-23 08:15:40.218] [controller_server]: [follow_path] [ActionServer] Client requested to cancel the goal. Cancelling.
[controller_server-12] [INFO 2024-11-23 08:15:40.219] [controller_server]: Optimizer reset
[controller_server-12] [INFO 2024-11-23 08:15:41.207] [controller_server]: Received a goal, begin computing control effort.
[controller_server-12] [INFO 2024-11-23 08:15:42.307] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:43.307] [controller_server]: Passing new path to controller.
[controller_server-12] [INFO 2024-11-23 08:15:43.912] [controller_server]: Reached the goal!
[controller_server-12] [INFO 2024-11-23 08:15:43.913] [controller_server]: Optimizer reset
[bt_navigator-16] [ERROR 2024-11-23 08:15:43.957] [bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for wait in node halt!
[bt_navigator-16] [INFO 2024-11-23 08:15:43.958] [bt_navigator]: Goal succeeded
[behavior_server-15] [INFO 2024-11-23 08:15:43.997] [behavior_server]: Canceling wait
[behavior_server-15] [WARN 2024-11-23 08:15:43.997] [behavior_server]: [wait] [ActionServer] Client requested to cancel the goal. Cancelling.

You can see that [behavior_server-15] [INFO 2024-11-23 08:15:40.197] [behavior_server]: Running wait
is stilled triggered although robot is rotating in place. However, this time, it does not take 10s like setting in my xml file <Wait wait_duration="10.0"/>. It only takes like 1 or 2 seconds to continue to rotate in place [controller_server-12] [INFO 2024-11-23 08:15:42.307] [controller_server]: Passing new path to controller..

Below is my launch command:
ros2 launch nav2_bringup tb4_simulation_launch.py use_gz:=False headless:=False use_rviz:=False use_composition:=False

[padhupradheep]

Analysing the issue, I see that BT is not inhibiting and causing the problem. @chanhhoang99 I see there seems to be a localization jump when the robot rotates near the goal location, which apparently causes the
"PathLongerOnApproach to be triggered, followed by the consequent behaviors. Could you please try adjusting the length factor? You have set it to 1.01, meaning, it's sensitive to localization jumps or another option would be to adjust the localization parameters if we are looking for cm level granularity.

Maybe you might need to add some behavior to the BT, that could check if the robot is within a specific goal tolerance, after which the PathLongerOnApproach should not be triggered. I think we have a behavior called, IsRobotInGoalVicinity that exactly does that. Something like the one I commented here: #4003 (comment)

@SteveMacenski what do you think? Localization jumps are something that we didn't consider for atleast the example BT that we have. Maybe open sourcing the solution that I showed for #4003 atleast might help @chanhhoang99 get through this issue.

[SteveMacenski]

Agreed!

[chanhhoang99]

Hi @padhupradheep , Thanks for the finding.
I will adjust the length factor and do the test.
I also want to have a try with IsRobotInGoalVicinity, is it available on nav2 official repository or it's from your fork open source work, could you tell ?

[padhupradheep]

Not yet, I'd update here, when we open source it (let me try to get it ASAP).

I also want to have a try with IsRobotInGoalVicinity

Note it is a condition plugin, therefore you would require to tweak around the BT to get your use-case working.

[padhupradheep]

Ah I see there is already a PR, that adds a similar behavior: #4620

@chanhhoang99 Would it be fine if I close this issue now ?

In essence, to solve the problem that you have posted, you need to tweak your BT such that, while reaching the goal proximity, the robot doesn't replan and trigger the PathLongerOnApproach.

[chanhhoang99]

Thanks @padhupradheep , I think you can close this issue.

[chanhhoang99]

Hi @padhupradheep , would IsPathValid help in this case ?
I have tried tweaking my BT with IsPathValid rather than replanning every 1s, the wierd behavior above is resolved.

Edit: it seems like with above behavior tree, after waiting, robot will choose the longer path which is valid instead of going the shorter path as I expect

[chanhhoang99]

With below xml, robot will replan again after waiting completely 10 second

However, with the original xml, robot do not have to wait 10 seconds.