Give behavior server access to both costmaps

[jwallace42]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	None
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo

---

Description of contribution in a few bullet points

This pr allows recoveries to have access to a local and global collision checker.

Description of documentation updates required from your changes

ros-navigation/docs.nav2.org#360

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@jwallace42, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[SteveMacenski]

Question:

Is it that you want to have both or that you want the option to instead use the global costmap? We should be able to make the behavior server work with either or pretty trivially. Going along with that, if you have some behaviors that want 1 and others that want the other, you could just launch 2 instances of the behavior server: once configured with global information, the other with local information. The costmaps + TF are the expensive things shared in the server, so if costmaps are actually different sets for different applications, I don't think there's any difference in performance in separating instances. But my feeling is that you really just need to be able to do either/or. Thoughts?

Also, there's still 1 global reference frame global_frame set to odom, if you want to use map for the global costmap information, you'd have to duplicate that too. That seems like its been overlooked in this PR.

There are unit tests failing that seem like it might be from this PR

[jwallace42]

Question:

Is it that you want to have both or that you want the option to instead use the global costmap? We should be able to make the behavior server work with either or pretty trivially. Going along with that, if you have some behaviors that want 1 and others that want the other, you could just launch 2 instances of the behavior server: once configured with global information, the other with local information. The costmaps + TF are the expensive things shared in the server, so if costmaps are actually different sets for different applications, I don't think there's any difference in performance in separating instances. But my feeling is that you really just need to be able to do either/or. Thoughts?

I need the behavior to have access to both the local and global the global costmap. Is there a better way to do this?

Also, there's still 1 global reference frame global_frame set to odom, if you want to use map for the global costmap information, you'd have to duplicate that too. That seems like its been overlooked in this PR.

Yes! I missed that :).

There are unit tests failing that seem like it might be from this PR

Yeah, this is a bit odd. When I tested locally I didn't have any issues.

[SteveMacenski]

Just a linting error on test failure now.

But the global frame stuff is still not there.

May I ask, is this for a single, specific behavior or for a series of behaviors? If this is a single behavior that needs both costmaps, I might say its better to just implement that as a separate server of its ownright rather than complicating this one. If its a number of behaviors that need both, then maybe its worth actually taking a bit of time for us to think about how we pass and work with parameter data for this server if its going to increase to multiple collision checkers / frames / footprints. Maybe having a behavior information struct containing a local / global struct of values, since they exactly mirror each other. E.g. my_info containing global_info and local_info each are instances of a struct info since they both involve costmap subs, footprint subs, and global frames (obviously not using those terrible names).

Potentially, we could ask the plugins what it wants (new method) for global/local so that we only subscribe to the ones required. Adding in a global costmap subscriber is really, non-trivial in cost, especially if no one is actually currently using it. There's a reason this server centralizes behaviors, even just streaming the local costmap at 5hz given its size is non-trivial. The server can have the logic for both, but only does the minimum required. based on if any of its behaviors desire it.

[jwallace42]

Just a linting error on test failure now.

But the global frame stuff is still not there.

Ahh!

May I ask, is this for a single, specific behavior or for a series of behaviors? If this is a single behavior that needs both costmaps, I might say its better to just implement that as a separate server of its ownright rather than complicating this one. If its a number of behaviors that need both, then maybe its worth actually taking a bit of time for us to think about how we pass and work with parameter data for this server if its going to increase to multiple collision checkers / frames / footprints. Maybe having a behavior information struct containing a local / global struct of values, since they exactly mirror each other. E.g. my_info containing global_info and local_info each are instances of a struct info since they both involve costmap subs, footprint subs, and global frames (obviously not using those terrible names).

It is for a series of behaviors that need both costmaps.

Potentially, we could ask the plugins what it wants (new method) for global/local so that we only subscribe to the ones required. Adding in a global costmap subscriber is really, non-trivial in cost, especially if no one is actually currently using it. There's a reason this server centralizes behaviors, even just streaming the local costmap at 5hz given its size is non-trivial. The server can have the logic for both, but only does the minimum required. based on if any of its behaviors desire it.

I think this sounds great since it will minimize changes to the current behaviors(and tests) while adding in the desired flexibility. If this is our path forward I can close this pr.

[SteveMacenski]

Why close it? Isn't it just adding in if condition and an API in the behaviors as to what they want?

[jwallace42]

That seems like a way we could go but I don't see that scaling well if we end up sharing more things across the behavior servers (maybe we don't). I was thinking about using https://en.cppreference.com/w/cpp/language/parameter_pack to feasibility pass different inputs to the concrete implementation. I have just starting looking into this so maybe it won't work :).

[SteveMacenski]

I'm not entirely sure what you mean, can you explain what you're thinking? I only suggested that rather than passing the behaviors a ton of objects, we just create a struct to manage them all + an API for each behavior to tell the server which, or both or neither, of the cost information they care about so we only make subscriptions to things strictly necessary.

[jwallace42]

Sure,

I was hoping we that parameters packs would allow us to configure the different servers to support a api as such:

configure(local_costmap, global_costmap, tf) needs 3 things
configure(local_costmap, tf) only needs two
configure(tf) just needs one :).

So,

you suggest that we have a struct that contains the necessary info that gets passed into the behavior though the configure func and the user selects what info to keep?

Or do we have multiple configure functions that support passing in only the necessary objects defined by some parameters? Similar to what I have above?

[SteveMacenski]

configure(local_costmap, tf)

The problem with that is how to differentiate between the local and global costmap if only one is given. How do they differ? I'll admit I'm not super familiar with parameter packs. It also doesn't entirely solve the problem because you still need to know which constructor to evoke for each given plugin, which would still require and API to query about what it wants. I think it would turn into something like

auto thing = plugin->tellMeWhatYouWant();
if (thing == local) {configure(local,  tf)} 
else if (thing == global) {configure (global, tf)}
else {configure(global, local, tf)} // both 

Which I don't think is any cleaner than just giving all 3 in 1 line to start with and let the behaviors decide what it wants to store and use. But I do think a plugin->tellMeWhatYouWant() can be useful to only instantiate the costmap/footprint subscriber and collision checker objects for the things that are required - which would have real efficiency gains if one or both of the costmaps weren't required. We can still pass in the same arguments since they're all shared pointers (so if not allocated, are just nullptrs) since we asked and know that none of the plugins will use that field.

I'm just suggesting the struct to manage the data better. But If you had a different design direction in mind that's better, I'm all ears. For how it is right now with just the local/global collision checker its fine without that. But I see this potentially getting more complicated once you have to add in the global/local reference frames (or it doesn't, I haven't gone through the process of thinking that out since you're on it 😉).

[jwallace42]

Let me know what you think of this solution. If we want to go with the struct it is a easy switch.

[SteveMacenski]

A couple of things but otherwise LGTM. Will obviously require docs updates for new params + migration guide

[SteveMacenski]

Last change here, then just need the docs PR for the new parameters and parameter name updates. Needs migration guide about this as well since we change parameters without backward compatibility - just to save you time by mentioning up front - please make sure that the Migration Guide changes are explicit about which parameters they need to change the names to to get the same behavior (since its not backward compatible).