collision_monitor: dynamic polygon and source enable/disable

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3824
Primary OS tested on	Ubuntu
Robotic platform tested on	Own gazebo simulation

---

Description of contribution in a few bullet points

• Added dynamic enabling/disabling of collision monitor polygons and sources
• Added test

Description of documentation updates required from your changes

---

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

[codecov]

Codecov Report

Attention: 1 lines in your changes are missing coverage. Please review.

Comparison is base (910a9f2) 90.33% compared to head (e12dc5e) 90.35%.
Report is 19 commits behind head on main.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3825      +/-   ##
==========================================
+ Coverage   90.33%   90.35%   +0.02%     
==========================================
  Files         413      413              
  Lines       18040    18087      +47     
==========================================
+ Hits        16296    16343      +47     
  Misses       1744     1744              

Files	Coverage Δ
..._collision_monitor/src/collision_detector_node.cpp	99.27% <100.00%> (+0.01%)	⬆️
...2_collision_monitor/src/collision_monitor_node.cpp	98.20% <100.00%> (+0.02%)	⬆️
nav2_collision_monitor/src/pointcloud.cpp	95.65% <100.00%> (+0.09%)	⬆️
nav2_collision_monitor/src/range.cpp	95.91% <100.00%> (+0.08%)	⬆️
nav2_collision_monitor/src/scan.cpp	97.22% <100.00%> (+0.07%)	⬆️
nav2_collision_monitor/src/source.cpp	97.36% <100.00%> (+2.63%)	⬆️
nav2_collision_monitor/src/polygon.cpp	97.37% <94.11%> (-0.24%)	⬇️

... and 1 file with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[SteveMacenski]

Minor changes but LGTM. Some test would be nice though, but I won't require it for this

[tonynajjar]

Do you think I should also skip publishing the polygon in publishPolygons if it's disabled?

[tonynajjar]

Also, since collision monitor and detector use the same Polygon, detector polygons will also have this new parameter. We could implement the same logic in collision_detector but I don't see much benefit to it except consistency/parallelism between the two nodes. What do you think?

[SteveMacenski]

Do you think I should also skip publishing the polygon in publishPolygons if it's disabled?

I think so! Anything else fall into this category?

Also, since collision monitor and detector use the same Polygon, detector polygons will also have this new parameter. We could implement the same logic in collision_detector but I don't see much benefit to it except consistency/parallelism between the two nodes. What do you think?

I don't see why not. Is it just adding the if not polygon->enabled() into the logic? If so, yeah, lets add it. Doesn't hurt anyone and really easy.

Make sure also to follow up on documentation updates to the migration guide + adding the new parameter to the configuration guide

[tonynajjar]

I think so! Anything else fall into this category?

Done, nothing else I see

Doesn't hurt anyone and really easy.

One drawback I see to disabling a polygon in collision_detector is that it would mess up the indexing of polygons in CollisionDetectorState msg; if some node subscribing to this topic has saved at initialization that a polygon is at a certain index, then disabling a polygon would cause this node to get the wrong index in detections (or even access an index out of range).

The reason why I said I see no value in disabling a polygon in collision detector is because if you don't want the status of a certain polygon, you just don't retrieve it.

If you still think that we should add the functionality and it's safe to expect that the user should always check if CollisionDetectorState msg has changed, then I'm fine with implementing it

[tonynajjar]

@SteveMacenski I also added the dynamic enabling/disabling of a source, please check it out

Make sure also to follow up on documentation updates to the migration guide + adding the new parameter to the configuration guide

Will do so once the scope is clarified

[tonynajjar]

I have a general question about dynamic parameter setting: from the Nav2 codebase I see that you prefer having these dynamicParametersCallback in which we set internal member variables with the new parameters.

Another way would be to directly read the parameter from the parameter server, roughly:

    auto node = polygon->node_.lock();
    if (node->get_parameter(polygon->getName() + ".enabled").as_bool()) {
         .....
    }

The benefit here is less boilerplate code. What's the con, is it slower? Or why do you prefer the first option?

[SteveMacenski]

if some node subscribing to this topic has saved at initialization that a polygon is at a certain index

That's really not good practice - we make no promises about the structure of that message array. Also, aren't only the polygons in collision published? If so, then there's no order to be assumed because there's no consistent polygon collision.

If you still think that we should add the functionality

I do - if the API exists for the base polygon/source type, we should use it anywhere applicable

dynamicParametersCallback in which we set internal member variables with the new parameters

Updating a parameter may require reinitializing other areas that parameter is being used in or you could be triggering an action mid-parameter update if trying to update many - so then the state of that action is not fully defined. Further, design wise, its not wise to inject ROS code into your deep algorithm logic, so getting the ROS params and converting to internal types lets you keep ROS out of the library cores + gives you control to explicitly accept or reject parameter updates if they're bogus or invalid, which happens.

In this particular library, in this particular case, it probably doesn't matter, but its the design pattern we use, so lets stick to them. I'm also not 100% sure how get_parameter() calls in a loop handle dynamic parameters. I don't necessarily think it will work?

[SteveMacenski]

@AlexeyMerzlyakov please review

[SteveMacenski]

@tonynajjar just needs migration guide entry for navigation.ros.org + configuration guide for the new parameters

[tonynajjar]

ros-navigation/docs.nav2.org#471

[SteveMacenski]

Great! Thanks! This is nice to have!