0
$\begingroup$

I'm trying to change Nav2's current planning and control pipeline from:

A* global planner at 1 Hz -> DWB controller at 10 Hz

To the following:

A* global planner at 1 Hz -> A* "local" planner at 10 Hz -> DWB controller at 10 Hz

Where the higher rate "local" planner uses the same A* algorithm as the global planner, but instead uses the local costmap and only re-plans the first few meters of the global planner's path.

I have tried to shoehorn this "local" planner idea into the existing controller_server.hpp/cpp files, but have so far had no luck (and this approach feels a little hacky). As such, I was wondering if there is a better approach to doing this? Should I instead be trying to add the local planner as an additional node that runs in the BT in parallel with the DWB controller? If so, I'm wondering how I might do that. Any insight would be appreciated, thanks.

EDIT:

Following some suggestions from Steve, I've tried to modify the existing navigate_to_pose_w_replanning_and_recovery.xml BT to use a global planner running at 1 Hz and a local planner running at 10 Hz:

<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}"/>
            <Sequence>
              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>
        <RateController hz="10.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="LocalGridBased" error_code_id="{compute_path_error_code}"/>
            <Sequence>
              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>
      </PipelineSequence>
      <Sequence>
        <Fallback>
          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
        </Fallback>
        <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" error_code_id="{spin_error_code}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.05" error_code_id="{backup_code_id}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

Furthermore, in navigation_launch.py I have added a local_planner_server to the list of lifecycle_nodes as follows:

lifecycle_nodes = ['controller_server',
                   'smoother_server',
                   'local_planner_server',
                   'planner_server',
                   'behavior_server',
                   'bt_navigator',
                   'waypoint_follower',
                   'velocity_smoother']
...
        Node(
            package='nav2_planner',
            executable='local_planner_server',
            name='local_planner_server',
            output='screen',
            respawn=use_respawn,
            respawn_delay=2.0,
            parameters=[configured_params],
            arguments=['--ros-args', '--log-level', log_level],
            remappings=remappings),

Where the local_planner_server is a slightly modified version of the planner_server that uses the local costmap instead of the global costmap, and uses a planner named LocalGridBased instead of the planner server's GridBased planner, which I've defined in nav2_params.yaml as follows:

local_planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    planner_plugins: ["LocalGridBased"]
    LocalGridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

However, it seems that the BT is unable to figure out if the ComputePathToPose running at 1 Hz should use the local_planner_server or the planner_server (and similarly for the ComputePathToPose running at 10 Hz).

As such I was wondering if it is possible to get the BT to know which planner it should run at 1 Hz and which planner it should run at 10 Hz. I had hoped that explicitly setting the planner_id field to GridBased from the planner_server and LocalGridBased from the local_planner_server would have given the BT an indication of which planner to use, but unfortunately this does not seem to be the case.

$\endgroup$

1 Answer 1

0
$\begingroup$

Sure thing, patterns like this are actually not uncommon. Usually in the context of a precomputed global route or when a route planner is used to give a general route to the goal and need a planner to create a dense path of that general route.

You could essentially have 2 planner servers, one with the global costmap and one setup with costmap params for a rolling costmap like the controller server has. Then cascade calling them - Planner server for the global route -> Planner server for the local path -> controller to track the local path. Then just setup the BT to set your desired rates, which should be straight forward.

$\endgroup$
4
  • $\begingroup$ Thanks for the speedy reply Steve, that sounds like a good solution. Just so I understand your proposal correctly, that would mean I would need to essentially create a local_planner_server.hpp/cpp that is very similar to planner_server.hpp/cpp (aside from the costmap changes, and maybe some goal changes). Then modify navigation_launch.py to include local_planner_server as a lifecycle node, and then modify the BT to include the local planner that uses a RateController of 10 Hz? Does that sound about right? $\endgroup$
    – indigoblue
    Mar 15 at 19:09
  • $\begingroup$ Just launch 2 instances. You dont need any new software. $\endgroup$ Mar 15 at 20:32
  • $\begingroup$ I've been thinking about your above comment for a little while and I'm not quite sure how I'd be able to get the global and local planners to interact correctly. I'd like the global planner to pass its global path to the local planner, and then have the local planner set its own local goal to be a waypoint along the global path. The local planner would then compute a local path to the local goal and output a modified global path that stitches the local and global paths together. This would then be passed to the DWB controller to follow. $\endgroup$
    – indigoblue
    Mar 15 at 22:01
  • $\begingroup$ Hi Steve, I've updated my question to include some additional information on how I'm currently attempting to run multiple planner servers in parallel and at different rates. If you have time to look over the issues I'm having with this approach that would be much appreciated. $\endgroup$
    – indigoblue
    Mar 16 at 2:44

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.