[collision monitor] Select the observation sources used with each polygon

[anaelle-sw]

Feature request

Feature description

Currently, the collision_monitor associates all the observation sources to all polygons.

In some cases, we need to apply only some of the observation sources, for some of the polygons.

Implementation considerations

A simple solution consists in launching multiple collision_monitor nodes, and split the observation sources (or polygons) definitions between these nodes. But it would require to define multiple times some of the polygons (or some of the observations sources). Plus, having multiple collision_monitor nodes it not necessarily a good design since it could induce latency in the velocity command.

A nicer solution consists in defining a new parameter <polygon_name>.observation_sources, for each polygon. This parameter would be a list of string, containing some (or all) of the names of the defined observation sources. Only the observations sources which names are listed in <polygon_name>.observation_sources will be used to check collision with the polygon <polygon_name>. The parameter observation_sources still exists and contains the names of all observation sources, and the sources definition is unchanged. If the list <polygon_name>.observation_sources is empty, all sources would be used (to no break previous usage). I can open an PR with an implementation of this solution.

[SteveMacenski]

In some cases, we need to apply only some of the observation sources, for some of the polygons.

Is it that you want to toggle certain sources for certain polygons, or is it more that you want to disable certain sources when the robot is in certain states (i.e. a box blocking this sensor on a forklift)? Currently, each source can be disabled with a dynamic parameter which you don't want to use that particular source at that moment.

Doing what you suggest is possible, but I think its going to be more ugly than you think, given how we pass just a data vector into the polygons that is quashed from all sources. We'd have to adjust the input to the polygon's functions to being individual data source vectors with their associated names rather than a vector of point values that have no attributes about their ownership (or, I suppose a vector of points with their name associations per-point). Not rocket science, but a little more involved since we need to propagate that source-ownership information to the polygons to be able to check with that parameter.

Can you motivate this feature a bit? I'm having a hard time imagining when this would be helpful, with the exception of optimizing performance by only checking certain sources over less number of polygons -- but my intuition is that is going to be negligible improvement to the full system -- but happy to be proved wrong

[anaelle-sw]

In this case, we don't really need to toggle sources while the node is up. Actually, we need to define at launch time which source will be checked with which polygon, for each polygon. Here is our use case for more context:

Our robot uses multiple ranges, a laser scan, and multiple point clouds data sources types. Because of robot's geometry and sensors position considerations, we would like to use certain sensors data to protect certain robot's parts. Typically, we want to use the point clouds to protect the whole robot's height (which can be up to 180cm). But the ranges and laser scan data would be no help upper than 20cm height. Since the robot's geometry is not necessarily the same below and above 20cm, the polygon geometry for "ground level" (0 to 20cm) may be different from the polygon geometry for "first level" (20 to 180cm), because we want the polygons to fit the robot's shape at best. So we need two polygons: a first one for "ground level" to check collision using the sources with types range, laser scan and point cloud, and a second one for "first level" to check collision only using the sources with type point cloud. I tried to describe our use case as simply as possible. But please tell me if you need more details or even a diagram.

About the implementation, yes I would need to add a new attribute to class Polygon, which would be a vector of string containing the names of sources associated to the polygon.
Actually, we have been using this feature for a few months now on our fork (which is based on this repo iron branch). To give you an idea of the required changes to make it work, I tried my best to keep compliance with nav2 coding standards and ~70 lines are modified in total, in four different files. The changes may be a bit different since the main branch would be targeted in this PR case, not the iron branch. I didn't actually check the feasibility on main branch, but the collision monitor node configuration and operation look similar enough on main and iron to not be a big problem in my opinion. I would be happy to discuss more about implementation details before opening a PR, if you think the feature worth the effort.

[SteveMacenski]

Please open the PR, we can discuss there - but seems OK.

[SteveMacenski]

To be merged imminently