[Collision monitor] Dynamic radius for circle polygon

[anaelle-sw]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4199
Primary OS tested on	Ubuntu
Robotic platform tested on	RobOcc's robot
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• New parameter <polygon_name>.radius_sub_topic, for circle type polygon. If the parameter <polygon_name>.radius is defined, it takes priority. If both <polygon_name>.radius_sub_topic and <polygon_name>.radius are not specified, an error is thrown. (Similar to points and polygon_sub_topic parameters in case of polygon-type polygons)
• New float-type subscription to <polygon_name>.radius_sub_topic, for circle type polygon. If a new radius message is received, the circle radius is updated

Description of documentation updates required from your changes

• Add new parameter <polygon_name>.radius_sub_topic to default configs, documentation page, migration guide: Collision monitor circle radius sub new param docs.nav2.org#538

---

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]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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]

@anaelle-sw only major missing thing I see is tests. You should be able to mimic the tests written for the dynamic polygons for the circles (and a little simpler)

Given these changes requested, also the docs should be updated in line with not having a new parameter + explaining the use in the existing parameter for a Circle vs Polygon type.

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[anaelle-sw]

only major missing thing I see is tests.

Ok, I will add some tests

Given these changes requested, also the docs should be updated in line with not having a new parameter + explaining the use in the existing parameter for a Circle vs Polygon type.

I updated the docs with the changes you suggested

[SteveMacenski]

Rebase / pull in main to get CI to turn over again 😄 Docs LGTM, I think a couple of tests is the only blocker.

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[anaelle-sw]

I added two tests for circle (based on existing tests for "polygon" type):

• check declaration of parameter polygon_sub_topic
• check update of circle radius when publishing a new message on topic polygon_sub_topic

[mergify]

@anaelle-sw, 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]

Please pull in main or rebase so that CI can turn over, then mark this as non-draft if its done and tested on your side

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[mergify]

@anaelle-sw, 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).

[anaelle-sw]

Some of the tests failed, so few changes were required:

• Initializing the variable radius_squared_ to -1 (will be updated either if the parameter radius is declared, or either when receiving a new message on radius topic)
• Updating the member function isShapeSet() for Circle class, so that it will return true only if radius_squared_ is not set to -1
• In member function getParameters, first try to get static polygon definition param (points or radius), and then call function getCommonParameters
• In member function getCommonParameters, declare topics param (polygon_sub_topic or footprint_topic), only if function isShapeSet() returns false (so only if static polygon definition param (points or radius) was not declared first)

[SteveMacenski]

Still have a few tests related to this PR failing in CI (and the docs PR is conflicted)

But otherwise once fixed, LGTM but will have to look at the final solution to see how these errors are fixed

[mergify]

@anaelle-sw, 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).

[anaelle-sw]

I fixed the tests failures by:

• Not setting a default value for parameter polygon_sub_topic. So if the user doesn't set it, an exception will be raised (and catch) and the parameter won't be declared.

• In case that the points parameter value is not pairs of number, clearing the polygon's points. In this case, the member function getPolygonFromString was already returning false, but if the polygon generation had already started, the partially-built polygon was kept as it was. Calling polygon.clear() prevents from using this partial polygon, and function isShapeSet() would return false.
  Since I had to implement next bullet point too, this one could actually be reverted and the tests shouldn't fail. But I thought that may be it wouldn't be bad to prevent usage of a partially built polygon (from parameter defined with format error).

• Finally, last failure source was the velocity polygon. During configuration, it gets a list of points for each of its sub polygon, but the variable polygon_ is actually not set until receiving a new velocity message. So the function isShapeSet() would return false until then, and when calling getCommonParameters the parameter polygon_sub_topic or footprint_topic would be declared, which is not expected.
  My first idea was to apply the sub polygon for null velocity range right after getting all points from all sub polygons. But I think it might induce unwanted behaviors, since the robot may not be actually stopped during configuration (it's not my case, but I rather not assume this).
  The second idea, which I implemented, is to add a new boolean argument use_dynamic_sub to member function getCommonParameters. When this argument is set to false, the parameters polygon_sub_topic and footprint_topic won't be declared. For velocity polygons, this argument is always set to false. For circles, this argument is set to true only if radius parameter is not declared. For polygons, this argument is set to true only if points parameter is not declared or is malformed.

With these changes, the tests for collision monitor finally succeed! Sorry this took so much time, I thought it was a bit less tricky. Are you ok with the updates?

[anaelle-sw]

I have also resolved the conflict of the docs PR

[SteveMacenski]

I'll admit that this PR is giving me a bit of a headache from no fault of yours. It just touches alot of very intertwined code and the diffs that GitHub show me aren't super helpful to really appreciate the difference - so I have to pull up files and compare manually.

I think this looks good to me but I'd appreciate it if @kaichie gave it a review as well to make sure I'm not missing an important detail for a second look. This is where the unit tests really come in handy that I generally know things work, but I'm not totally confident I am juggling all of the edge cases for this one in my head so a second pair of eyes is necessary

I see that you added use_dynamic_sub instead of clearing the string and checking if its empty - not that I need you to justify it, but why wasn't checking empty string sufficient?

[anaelle-sw]

I see that you added use_dynamic_sub instead of clearing the string and checking if its empty - not that I need you to justify it, but why wasn't checking empty string sufficient?

If static polygon is used (which means parameter points is properly declared), the parameters polygon_sub_topic and footprint_topic are supposed to not be declared (or at least, this is the expected behaviour in tests, may be this can change...?). So with current design, there are two solutions that I can think of:

• undeclared parameters polygon_sub_topic and footprint_topic if static polygon is used. I tried this, but dynamically typed parameters cannot be undeclared, so this doesn't build
• use an new argument (use_dynamic_sub) to not declare parameters polygon_sub_topic and footprint_topic if not necessary

I agree adding a new argument is not very nice. Any other solutions you can think of with current design?

Or we could change design. For instance, declaring
polygon_sub_topic and footprint_topic in getParameters (instead of getCommonParameters ), or considering the sub topic parameters can be declared in case the static polygon is used.

[kaichie]

Overall the changes looks good to me and seems to follow the existing pattern. Happy to see we can now dynamically update the Circle polygon! 👍 🌟

[SteveMacenski]

OK! Any reason I shouldn't merge this on Monday @anaelle-sw ?

[anaelle-sw]

OK! Any reason I shouldn't merge this on Monday @anaelle-sw ?

I'll just rebase branch on main and it should be ready to be merged