CM/CD: Add polygon source

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom gazebo simulation

---

Description of contribution in a few bullet points

We would like to add a source to the collision monitor and that is an array of arbitrary PolygonStamped fed from a topic. Our specific use case is that we would like to prevent collisions with some tracked objects.

Note: Adding these tracked objects to the global/local costmap was tried, however this was not enough as the controller makes a "best effort" to avoid the collisions; we need more decisive logic to stop the robot and therefore need the collision monitor

• Add Polygon as a source
• Create new msg type PolygonArray to feed in polygons to the Polygon source

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

[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]

@SteveMacenski @AlexeyMerzlyakov that's a first draft, a lot of small improvements can still be made but wanted your opinion on the idea before spending more time refining it. Thanks

[SteveMacenski]

Test coverage?

I'd think for polygon-source types, we would want to make the minimum number of points 1 for this special case

[tonynajjar]

I thin most points have been addressed, please have another look.

[SteveMacenski]

@AlexeyMerzlyakov please re-review

@tonynajjar this file has the most missing coverage of the Collision Monitor files https://app.codecov.io/gh/ros-planning/navigation2/blob/add-polygon-source-main/nav2_collision_monitor%2Fsrc%2Fpolygon_source.cpp#L179 please fill in the gaps. I'm not worried about the node.lock() exceptions, but there is a complete function missing and a for loop unexecuted. This particular package more than all others needs to have really solid test coverage due to its nature. Add a few more things to really exercise and check it all, this one is pretty key

At this point, the rest of the code here is approved except the actual polygon_source hpp/cpp's implementation of the polygon deconfliction

[SteveMacenski]

Generally looks good, but I don't think the are polygons similar approach is valid general approach to this. This assumes alot about what specifically these polygons are, how often they're published, and that they're spatially near each other due to that regular frequency

[SteveMacenski]

@tonynajjar when you're back, rebase and fix the API changes for bool getData and checking the timeout via #3880 merging

[AlexeyMerzlyakov]

The main challenge here - is how to solve moving polygon task, where polygon changing its coordinates during the time.
Whether should we use unique_identifier_msgs/msg/UUID.msg (in newly added nav2_msgs message or by resolved ros2/common_interfaces#230 task); or get back to previous implementation when all polygons are being published once per a time, or might be something else (which I did not note).

[tonynajjar]

Addressed most of the comments, I'll wait for the result of ros2/common_interfaces#230 to continue this PR and the tests. It's also my preferred solution to the problem

[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).

[SteveMacenski]

@tonynajjar Polygons with IDs have been added! ros2/common_interfaces#232 (comment)

That can be used now to implement this PR to get over the issue :-)

[tonynajjar]

Awesome! In the meantime I left my old company that needed this and am quite busy with my new one. @jplapp maybe you want to take over? If not I'll try to eventually find the time but can't promise when.

[SteveMacenski]

It should be straight forward now if using those messages. All of the arePolygonsSimilar functionality can be deleted and just check the IDs. Add a few unit tests and a nice video and I think its done.

[jplapp]

Thanks for review! Sure, I'll find someone to complete this, but cannot promise a timeline

[oldtopos]

@jplapp I have cycles to pick this up; it seems there is one use of arePolygonsSimilar to replace, so this should be straightforward. LMK if that works for you

[oldtopos]

I am attempting to copy the branch associated with the PR to my own fork of nav2 in github, as I presume (and have not tested) I do not have commit access to the repo currently associated with the merge. Will see how the build goes tomorrow, fingers crossed

[mergify]

This pull request is in conflict. Could you fix it @tonynajjar?

[SteveMacenski]

@oldtopos did you happen to get a chance to play with this? It was largely done and it would be a shame to let this die on the vine if you'd get use from it

[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).