nav2_collision_monitor: collision detector

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3184
Primary OS tested on	Ubuntu
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)

---

Description of contribution in a few bullet points

This PR introduces the Collision detector, a node similar to the Collision Monitor in the sense that it detects obstacles in regions, with the difference that the CD does not affect the velocity directly, but instead publishes the detections to a topic for other Nodes in the system to take actions

Description of documentation updates required from your changes

---

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

[tonynajjar]

Happy to hear your opinions @SteveMacenski @AlexeyMerzlyakov. There could potentially be overlap with #3496

[SteveMacenski]

From a quick glance, I think the discussed solution(s) in #3496 are better fits. This seems less contextual with just the boolean and also a bit odd to have it be an "action" like slow/stop are actions. I think it would be sensible to have this streaming state at its natural frequency (even with Nones) which makes the added state for "publish" not really relevant.

Or if not published at the natural rate, we already have a check for printAction if the state changes, we can change that to notifyActionChanges and log + publish. No need to embed this deeper into the polygons or the action processing logic 😄

[tonynajjar]

#3496 would not fully satisfy my requirements. I need a polygon with an action that will not affect cmd_vel, maybe the DO_NOTHING action could be used for it? Then by publishing the "state" of collision monitor as explained in the #3496, I would get the information whether my DO_NOTHING polygon is triggered or not.

One more requirement I have is that this DO_NOTHING polygon should run based on a timer and not in the subscription of cmd_vel (We want to check for a "collision" while the robot is stationary). I workaround it currently by doing exactly that, creating a timer and processing the DO_NOTHING/PUBLISH polygons in it. (not shown in this PR). I don't love this workaround though

[AlexeyMerzlyakov]

@tonynajjar, I think the best solution here should be to add new <polygon_name>.do_nothing ROS2-option to the polygon properties. This change plus #3496 should meet the Req.1 (Just report state when polygon was triggered). This is much more unified solution, since it will also cover the situations when we need to track "approaching to the obstacle" state changes, or the case when we have to set a specific polygon with minimal number of points triggering the specific action.

Regarding Req.2 (publishing the state on regular rate), it could be done either by CM or by operating S/W side. However, from the performance perspective, CM is better to publish the state one time since it was changed (the same as all other Nav2 nodes currently do). Adding the rate to the CM, I do not consider as a good choice since: 1. as mentioned above, performance reasosns, 2. in some cases, operating S/W should know as soon as possible the state change. You can add the message callback, and set internal state in your application to "on"/"off"; and then frequently query this internal state, e.g. on regular rclcpp::Rate basis in this case.

[SteveMacenski]

@tonynajjar this seems like you need a completely separate addition to this server, if it makes sense to add here. Can you provide some specific information about what you're trying to do so it can help drive our mental model of how to help?

If you just need published on a timer, we could just create a timer to publish separately of any of this within the server. I don't understand what it is you're looking for (or why) to give a more concrete suggestion. But I don't think adding another polygon type for "do nothing" is a great answer (but might be right?)

[mergify]

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[tonynajjar]

ros-navigation/docs.nav2.org#451

[tonynajjar]

https://app.circleci.com/pipelines/github/ros-planning/navigation2/9813/workflows/3fdd3100-a4b0-46c0-ab3f-881de07fb1f3/jobs/31374/parallel-runs/0/steps/0-117

How do I solve this? Does it have to be called main.cpp? @SteveMacenski

[SteveMacenski]

[0.000s] [error]: Cannot find '/opt/overlay_ws/src/navigation2/nav2_collision_monitor/src/main.cpp'

I think you just renamed the files so the cache isn't quite right. If you were to increase the v14 (or maybe v15) in the circlci file (there should be 3 instances) then that'll create a new cache for the change in file locales that its looking for.

[tonynajjar]

[0.000s] [error]: Cannot find '/opt/overlay_ws/src/navigation2/nav2_collision_monitor/src/main.cpp'

I think you just renamed the files so the cache isn't quite right. If you were to increase the v14 (or maybe v15) in the circlci file (there should be 3 instances) then that'll create a new cache for the change in file locales that its looking for.

bumped from v15 to v16, let's see

[SteveMacenski]

Works now, just retriggering due to a (increasingly annoying) flaky test - since it would be good to see the actual code coverage report pop up. Though you can find it here: https://app.codecov.io/gh/ros-planning/navigation2/tree/collision-monitor-publish-action-main/nav2_collision_monitor%2Fsrc

It looks like you could cover a couple of extra things that would button it up. Normally for just missing a few lines I wouldn't care (since the last few lines are usually the hard ones) but the ones you have missing are pretty straight forward to test:

• Use a "circle" type for a dummy test, not just polygon
• Incorrectly set to a type other than DO_NOTHING to test your error condition that it should fail -- actually that hsouldn't just log a warning that should fail the configuration since something is critically wrong!
• Test pointcloud source, not just range and scan

These just need to be minimal tests just to exercise things, since it shares so much with the collision monitor.

[tonynajjar]

Use a "circle" type for a dummy test, not just polygon

hmm, it's weird that the coverage shows that a Circle creation is never tested, it should be in the test called testCircleDetection

actually that hsouldn't just log a warning that should fail the configuration since something is critically wrong!

okay fixed in latest commit

Test pointcloud source, not just range and scan

I tried that for 2 hours, the test just kept failing in CI but worked locally so I gave up. Giving it another shot in the latest commit

[tonynajjar]

Tests pass 🎉 @AlexeyMerzlyakov could you do a final round of review here and on ros-navigation/docs.nav2.org#451? Thanks

[SteveMacenski]

I'm happy if Alexey is

[AlexeyMerzlyakov]

All review items were resolved by now. Local nav2_collision_monitor testing passes. collision_detector_node.cpp coverage = 99.4% with total code coverage for CM package = 97.4% which is great.
I am OK to merge this change to master (docs are in review now, but to be on finish line).

[tonynajjar]

Thanks for the reviewing iterations on this

[SteveMacenski]

Thanks for sticking with it :-)