Feature Request（galactic）: RecoveryServer use it's own costmap for collision check

[cf-zhang]

Feature request

recovery server should have it's own costmap for more flexible.

Feature description

Requirements are described as follows

1. set global costmap && local costmap's frequency to 0.0,and use updateMap() for manual update.
2. then recovery server can not subscribe CostmapTopicCollisionChecker.
3. backup or spin recoveries would failed when exception occured.

Implementation considerations

[jwallace42]

Can you elaborate on why having a duplicate costmap in the recovery server would improve flexibility?

What exception are you taking about?

[padhupradheep]

adding on to the questions posted by @jwallace42

set global costmap && local costmap's frequency to 0.0

how would the global and the local planner provide sensible plans ?

As @jwallace42 pointed out, an use-case would be helpful!

[cf-zhang]

I modified the planner server and controller server so that they can specify a different Costmap for each planner (for example, the path planning for target tracking requires the rolling window property on global costmap; the local planners requires costmaps of different sizes).

in order to fully utilize the cpu， it is planned to set the refresh rate to 0.0 for costmaps, and trigger it update manually when planning or controlling. so I replaced costmap_ros_->isCurrent() with costmap_ros_->updateMap() in planner_server and controller_server @padhupradheep

when I do this, I found that:

1. When the frequency of the map is set to 0.0, the map will not work properly because the initialized_ is not set.
2. When performing the fallback recovery behavior, it will fail, because it's collision checker is subscribed from local costmap but it did not update for frequency 0.0 .

When I encountered these problems and tried to solve them, I came up with the idea of giving the Recovery server a separate costmap @jwallace42

[jwallace42]

So,

the recovery server is listening to the local costmap. What you have described is changes only to the global costmap which is a completely separate entity from the local costmap. Did you make adjustments to the controller server?

What do you mean by path planning for target tracking?

Overall,

I am very confused about what problem you are trying to solve. So I think it is best that you explain the problem you are trying to solve at a high level.

[SteveMacenski]

I modified the planner server and controller server so that they can specify a different Costmap for each planner

I think your core issue there is that you could have / should have instead created multiple instances of the planner and controller servers if you were going to do that. There's no benefit of having them all in the same node if you're going to maintain separate costmaps anyway. Could have saved you a bunch of time too in coding :-)

You're right that if you don't publish anything on the costmap topics, then the behavior server will have no representation to use. One such option to that is to modify the behavior server to have its own costmap, which comes at a pretty large cost. The other 2 options are:

• Have the behavior server have an "optional" costmap or subscribe to a topic, either or. That's an "OK" solution, but involves more coding
• You as the user can simply create an instance of the costmap node and then feed that costmap into the behavior server. There's already a costmap standalone node you can use. That's a zero-code solution that maintains flexibility. When using composition (default on), there's very little overhead that having it in a seperate node proposes.

Overall, your suggestion is good, but your have actually all of the tools at your disposal to do what you're suggesting without any new code and just modifying launch files.

• Create N instances of the planner/controller server to your heart's desire if you need multiple costmap configurations
• Create an instance of the costmap node for the behavior server if you need a separate instance for that + feed the updated topic to the behavior server's configuration.

Separately, your idea of setting the update rate to 0 and triggering an update only when required is fine enough, though you will drop / miss out on information if your planning/control frequency for requesting those updates is too long. If you only replan every 10 seconds but your sensor timeout is 2 seconds, then you'll be dropping the last 8 seconds of useful information to populate the costmap over. For some situations, that could be fine, but you're reducing the information in your actual costmap by doing so giving the robot a worse understanding of the world. Plus, for the controller, you're tying the costmap update rate to the controller rate, which could be very large (100hz+) when it doesn't really make sense to update the costmap at that speed, which would actually cost you much more CPU than if you let it just update in the background.

In conclusion: good idea, here are some suggestions that would probably be better for you. I'm planning on closing this since I think this is a 'pretty good' and complete answer to you request that would involve much less work. However, I'm totally happy to reopen and continue discussion if that does not satisfy you or you believe there are some changes / options we should add to the controller/planner servers (like maybe the option to only update on request).

[SteveMacenski]

@cf-zhang I filed a ticket https://github.com/ros-planning/navigation2/issues/3121 around adding an option to only update the costmap when a request comes in as you did in the comments above.

Would that be something you'd be open to contributing? That + the suggestions above would put you back in line with Nav2 source and you wouldn't have to maintain a fork.

[cf-zhang]

I modified the planner server and controller server so that they can specify a different Costmap for each planner

I think your core issue there is that you could have / should have instead created multiple instances of the planner and controller servers if you were going to do that. There's no benefit of having them all in the same node if you're going to maintain separate costmaps anyway. Could have saved you a bunch of time too in coding :-)

You're right that if you don't publish anything on the costmap topics, then the behavior server will have no representation to use. One such option to that is to modify the behavior server to have its own costmap, which comes at a pretty large cost. The other 2 options are:

• Have the behavior server have an "optional" costmap or subscribe to a topic, either or. That's an "OK" solution, but involves more coding
• You as the user can simply create an instance of the costmap node and then feed that costmap into the behavior server. There's already a costmap standalone node you can use. That's a zero-code solution that maintains flexibility. When using composition (default on), there's very little overhead that having it in a seperate node proposes.

Overall, your suggestion is good, but your have actually all of the tools at your disposal to do what you're suggesting without any new code and just modifying launch files.

• Create N instances of the planner/controller server to your heart's desire if you need multiple costmap configurations
• Create an instance of the costmap node for the behavior server if you need a separate instance for that + feed the updated topic to the behavior server's configuration.

Separately, your idea of setting the update rate to 0 and triggering an update only when required is fine enough, though you will drop / miss out on information if your planning/control frequency for requesting those updates is too long. If you only replan every 10 seconds but your sensor timeout is 2 seconds, then you'll be dropping the last 8 seconds of useful information to populate the costmap over. For some situations, that could be fine, but you're reducing the information in your actual costmap by doing so giving the robot a worse understanding of the world. Plus, for the controller, you're tying the costmap update rate to the controller rate, which could be very large (100hz+) when it doesn't really make sense to update the costmap at that speed, which would actually cost you much more CPU than if you let it just update in the background.

In conclusion: good idea, here are some suggestions that would probably be better for you. I'm planning on closing this since I think this is a 'pretty good' and complete answer to you request that would involve much less work. However, I'm totally happy to reopen and continue discussion if that does not satisfy you or you believe there are some changes / options we should add to the controller/planner servers (like maybe the option to only update on request).

Thank you very much for your advice， I understand what you said is convenient for create multiple instances of the planner and controller servers. And I tried it before I proposed this request, but was confused by action servier's name for action nodes in the behavior tree. I modified the planner server and server so that each plugin use costmap for its own with parameters like this:

planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0
    use_sim_time: True
    planner_plugins: ["GridBased", "SplinePlanner", "PotentialPlanner", "SplinePlannerL"]
    planner_costmaps: ["rolling_costmap_charging" , "rolling_costmap", "global_costmap"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      costmap: global_costmap
     ...
    SplinePlanner:
      ...
      costmap: rolling_costmap
      ...

and it works well for various operation scenarios.

Thank you again for your detailed description on this question :grinning: