Skip to content

Switching between Planner Plugins in the Behavior Tree #5118

@ipa-dmo

Description

@ipa-dmo

Required Info:

Operating System:
    Ubuntu 24 (in docker container)
ROS2 Version:
    Jazzy
Version or commit hash:
    jazzy
DDS implementation:
    Cyclone
Nav2 Package:
    - nav2_behavior_tree
    - nav2_planner
    - nav2_smac_planner
Have you asked this on Robotics Stack Exchange?
    No

Hello,
we have a use case, where we want to drive with two different planner configurations/plugins. For some navigation tasks we want to allow the robot to plan and drive backwards (it could be needed as well to reach the goal) but for other following navigation tasks we want to disable it and force e.g. a minimal turning radius.

I created an small minimal example branch here.

I would assume something like the following BT configuration (link: here) would allow such a behavior.
The "GridBased" planner ID does not allow backward driving, but the "GridBased_reversing_enabled" does allow it. See nav2_params here

<PipelineSequence name="Navigate_4_goals">
  <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
  <GetGoalFromGoalsVector goals="{goals}" goal_index="0" goal="{goal}"/>
  <GetGoalFromGoalsVector goals="{goals}" goal_index="1" goal="{goal_2}"/>
  <GetGoalFromGoalsVector goals="{goals}" goal_index="2" goal="{goal_3}"/>
  <GetGoalFromGoalsVector goals="{goals}" goal_index="3" goal="{goal_4}"/>
  <PipelineSequence>
    <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
    <ComputePathToPose goal="{goal_2}" path="{path}" planner_id="GridBased_reversing_enabled" error_code_id="{compute_path_error_code}"/>
    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
    <ComputePathToPose goal="{goal_3}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
    <ComputePathToPose goal="{goal_4}" path="{path}" planner_id="GridBased_reversing_enabled" error_code_id="{compute_path_error_code}"/>
    <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
  </PipelineSequence>
</PipelineSequence>    

Would this be in general a viable approach?

When running the example branch with ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False (replaced the default navigate through poses BT navigator to this BT configuration) with 4 Goals, as shown in the next screenshots, the robot allows backward driving for all intermediate goals after the first one.
Not only for the paths between 1->2 and 3->4 but also for 2->3, where backward driving should not be possible due to the DUBIN motion modell.

Defining 4 Goals in directions, which would trigger backward driving.
Image

Driving to the first goal:

Image

Driving to the second goal: Robot performes the allowed backward driving.

Image

Driving to the third goal: Robot still plans a path backwards, even though its planner_id should not allow this.

Image

Driving to the fourth goal: Robot performes the allowed backward driving.

Image

But it does not only effect this behavior tree, but also future planing after successfully executing this behavior tree. When sending afterwards a new normal goal for the navigate_to_pose action, the robot still uses backward driving, even though the navigate_to_poaw action should use the "GridBased" planner instance with the DUBIN motion modell:

Image

In contrast, when sending a goal before executing the navigate_through_poses actions the planner does limit itself to forward driving.

Somehow for me it looks like, the planner ID does not update correctly inside the planner server.

Metadata

Metadata

Assignees

No one assigned

    Labels

    enhancementNew feature or request

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions