AMRNAV-3826 Add dynamic parameters to nav2 collision monitor

[HovorunB]

---

Basic Info

Info	Add dynamic parameters to nav2 collision monitor
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)

---

Description of contribution in a few bullet points

• Added dynamic configuring of the parameters pointcloud.max_height, pointcloud.min_height
  Related PR: https://github.com/logivations/deep_cv/pull/4793
  We need these parameters to be dynamicaly configurable in order to turn on/off the collision monior by setting the parameters to some unrealistic values like poincloud.min_height = 10. SetNodeParameters BT node is responsible for this

---

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

[jplapp]

@tonynajjar please have a look

[HovorunB]

Copied from ros-navigation#2922

[tonynajjar]

As I understood it, this is to turn off the collision monitor completely, and not to "tune" these parameters. Then I propose another approach. @jplapp we once discussed the Multiplexer design for selecting the source of the cmd_vel topic, i.e if it comes from nav2 or from joystick control (according to driving mode). I think this design would also fit quite well here; we select if the cmd_vel comes from the collision monitor or from the step before it (the smoother i think)

Pros:

• no changes to nav2
• semantically better then setting a high min distance to turn it off
• gain experience with this design as we will likely reuse it

@HovorunB @jplapp what do you think?

[HovorunB]

The multiplexer design also sounds good for me, the only problem i see is that it adds one more layer to our cmd_vel latency question

[tonynajjar]

The multiplexer design also sounds good for me, the only problem i see is that it adds one more layer to our cmd_vel latency question

really good observation. My gut feeling is that it's not an extra layer is going to make it or break it, if sub/pub layers really bring unbearable latency then we would have to rethink the Nav2 communication pipeline as a whole.

[jplapp]

thanks for you comment @tonynajjar . I'd be fine with this design as well, however the min_height does have some practical use in the application - we don't want to fully turn off collision monitoring. Instead, we just don't want the ground to be an issue:
The problem we have is that the 3d camera can see the ground when driving from an inclined area onto the standard ground. There might still be other obstacles on that normal ground (e.g. other AMRs with raised forks), so, tuning the min_height so that the ground is removed for some area in front of the AMR, but so that other obstacles would still be visible, would be quite nice in our usecase.

[HovorunB]

Sorry for the confustion, i understood the task wrong, i thought we want to turn it off completely. I'm thinking if the default value of 5cm min_height would be enough to leave the inclined area with no issues, we can test it and tune the min_height at Brummer, and then decide if we need to dynamicaly change this parameter or we can stick to the value that is enought to leave the inclined area, make it a default and revert this PR

[tonynajjar]

Instead, we just don't want the ground to be an issue

I see, but then I think this should be solved at the earlier stage since a "pointcloud without ground" is also to be used for STVL. (currently we work around this also with the min distance If I recall correctly)

[jplapp]

currently we disable STVL - if it had dynamic param support and we could modify the min_distance in the same way, that would be nice as well
That said, we could also dynamically add a new plane to amr_plane_filter, that should then catch both places

[HovorunB]

Merging this as it is for now. We will test it at Brummer to tune the parameter min_height. Also created https://lvserv01.logivations.com/browse/AMRNAV-4062 to move this logic to plane_clipper