[for help] BT node Is_path_longer_on_approach doesn't work

[kevinpretell]

Bug report

I'm practicing on this nav2 simulation for outdoor navigation without problems.
https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html

To manage obstacles I found this BT node which is very interesting, if the robot sees a dynamic obstacle it stops until it goes away.
https://navigation.ros.org/behavior_trees/trees/nav_to_pose_and_pause_near_goal_obstacle.html

I then replaced my default nav_to_pose.xml behavior with the one from the demo above by placing it in the config folder.

But nav2 seems to ignore the new beaviour xml by following its path without stopping at the obstacle.

I ask here because I saw that no one is talking around about this BT node.
Many thanks in advance

• Operating System:

  • Ubuntu 22.04

• ROS2 Version:

  • ROS2 Iron

• DDS implementation:

  • default dds

Steps to reproduce issue

Replace the default bt: navigate_to_pose_w_replanning_and_recovery.xml
with the bt: navigate_to_pose_w_replanning_goal_patience_and_recovery.xml

PathLongerOnApproach setting:
<PathLongerOnApproach path="{path}" prox_len="6.0" length_factor="1.2">

start the gazebo simulation:
ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True

From the gazebo I add an obstacle in front of the moving robot.

Expected behavior

the robot should stop and wait for the obstacle to go away before following the new path

Actual behavior

the robot does not stop when a longer path is recalculated waiting for the obstacle to go away, and takes the longer path

Additional information

Maybe I need to change something else in the Nav2 params file?
here is my bt navigator params:

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    default_nav_to_pose_bt_xml: "/home/kevin/iron_ws/src/nav2_gps_waypoint_follower_demo/config/nav_to_pose.xml"
    default_nav_through_poses_bt_xml: "/home/kevin/iron_ws/src/nav2_gps_waypoint_follower_demo/config/nav_through_pose.xml"

    # 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.
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_are_error_codes_active_condition_bt_node
      - nav2_would_a_controller_recovery_help_condition_bt_node
      - nav2_would_a_planner_recovery_help_condition_bt_node
      - nav2_would_a_smoother_recovery_help_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

here is my new default_nav_to_pose_bt_xml:

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" 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="6.0" length_factor="1.2">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="5"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="FollowPath" 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"/>
          <BackUp backup_dist="0.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

[SteveMacenski]

Your BT probably isn't being changed, did you verify that you are actually using the new BT or see any log errors?

Please ask on Robotics Stack Exchange unless @padhupradheep has any insights to share

[padhupradheep]

Hey @kevinpretell

I see that you are using a proximity length of 6m with a factor of 1.2 m. The length of the new plan does not seem to be (significantly) longer based on the video that you have posted.

[padhupradheep]

Okay, we need to re-open this issue I think. Just noticed that, SequenceStar is replaced by SequenceWithMemory in the BTCpp library.

Edit: we need to take care of it while update to BT 4. Just looked at source of the BT. For some reason, the documentation of BT.CPP does not reflect that in V3.8.

[padhupradheep]

Alright, in any case, it doesn't work @kevinpretell thanks for reporting. Sorry for overlooking it before, as you might know, we are very low in resource and try our best to support you as much as we can.

I've also taken the opportunity to go one step ahead by fixing the issue.

@SteveMacenski: could you please reopen the issue, before I can give in the fix? I still need to test it.

Crux of the issue seems that the status of the node is set to IDLE. Last year, when I had implemented, the status ended up to success and went IDLE only when the entire BT was IDLE. This IDLE state was blocking the logic of the intended decorator going forward. I however have found a fix, that uses the goals of the old path and the new path, to avoid the blocking.

[BCKSELFDRIVEWORLD]

I'm really looking forward to learning about your progress in the development process and how you've found a solution to this problem. Best wishes for your continued success and great efforts as you conclude this work.@padhupradheep

[deepak-anzo]

I am also facing similar problem. I need a behaviour tree that doesn't replan and wait for the obstacle to clear infinitely. It only planes when the new goal is received. Please help.

[ambrosekwok]

Alright, in any case, it doesn't work @kevinpretell thanks for reporting. Sorry for overlooking it before, as you might know, we are very low in resource and try our best to support you as much as we can.

I've also taken the opportunity to go one step ahead by fixing the issue.

@SteveMacenski: could you please reopen the issue, before I can give in the fix? I still need to test it.

Crux of the issue seems that the status of the node is set to IDLE. Last year, when I had implemented, the status ended up to success and went IDLE only when the entire BT was IDLE. This IDLE state was blocking the logic of the intended decorator going forward. I however have found a fix, that uses the goals of the old path and the new path, to avoid the blocking.

@padhupradheep

I also found this problem with ros2 humble, are you found a fix by modifing navigate_to_pose_w_replanning_goal_patience_and_recovery.xml?
Could you share more how your fix work?

Thank you.

[padhupradheep]

Please give me few more days, I will do the fix..

Really sorry for the long delay...

[SteveMacenski]

Resolved by the PR linked above! Thanks for the report @kevinpretell

[padhupradheep]

reopening, doesn't work with the smac planners.. headers have proper stamps while the solution that was given for this was developed only for the NavFN (which doesnt have stamps). Let it be assigned to me, I'll fix it.

And also not to forget the orientations..