Add velocity based polygon

[kaichie]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#3313
Primary OS tested on	(Ubuntu)
Robotic platform tested on	Nav2 TB3 /Dexory gazebo simulation

---

Description of contribution in a few bullet points

• Adds velocity based polygon as a new Polygon type in Collision Monitor

Description of documentation updates required from your changes

• Added new polygon_velocity type, so need to add that to default configs and documentation page(configuration, tutorial + migration)

---

Future work that may be required in bullet points

• Add odom speed condition check (linear, direction + theta too) for speed based polygon. (Better control on when to switch based on actual speed)

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

[AlexeyMerzlyakov]

Overall approach seems good for me.

The rotation at the place could be handled by making small polygon inside when the linear velocity is equal to 0.0 (+ epsilon).

Another point, that developer could intentionally leave some ranges in the velocities, where no one polygon_velocity will be applied. This case should be tracked (informed, or warned), but I do not see any reason to check all the velocity ranges in {linear, angular} space preliminary for the consistency before we will apply the rules.

The question with overlapped polygons_velocity for one polygon remains to be discussed: should we find safest polygon_velocity responsing to the input velocity, or agree that velocity ranges within one polygon shouldn't be overlapped?

Please note, that circles will not have an ability of polygons_velocity utilization (which is OK from my point of view, and already handled by this change as polygons_velocity_ vector is not being initialized in the constructor).

There are some changes to be noted in the review items below, please refer to them

[codecov]

Codecov Report

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

Comparison is base (a8a0c3a) 64.68% compared to head (898d8c0) 90.06%.

❗ Current head 898d8c0 differs from pull request most recent head 680f7ca. Consider uploading reports for the commit 680f7ca to get more accurate results

Files	Patch %	Lines
nav2_collision_monitor/src/velocity_polygon.cpp	0.00%	76 Missing ⚠️
..._collision_monitor/src/collision_detector_node.cpp	25.00%	3 Missing ⚠️
...2_collision_monitor/src/collision_monitor_node.cpp	40.00%	3 Missing ⚠️

Additional details and impacted files

@@             Coverage Diff             @@
##             main    #3708       +/-   ##
===========================================
+ Coverage   64.68%   90.06%   +25.37%     
===========================================
  Files         415      416        +1     
  Lines       18701    18732       +31     
===========================================
+ Hits        12097    16871     +4774     
+ Misses       6604     1861     -4743     

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

[kaichie]

Another point, that developer could intentionally leave some ranges in the velocities, where no one polygon_velocity will be applied. This case should be tracked (informed, or warned), but I do not see any reason to check all the velocity ranges in {linear, angular} space preliminary for the consistency before we will apply the rules.

@AlexeyMerzlyakov Regarding of this point, the warning(on run-time) of uncovered velocity is added in this commit. If the user is intentionally leaving some ranges in the velocities, then this might conflict with what we were discussing in the issue to check the full range velocity(@SteveMacenski). We might need to specify the intended gap(s) in the config but I think this will greatly increase the complexity of setting it up.

[SteveMacenski]

I commented back on the main ticket thread on that topic. Maybe just logging is enough if we provide good example profiles with the documentation

[SteveMacenski]

Blah blah blah, the usual bits on documentation (readme + configuration guide + migration guide) and test coverage :-)

[mergify]

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

[SteveMacenski]

Any update? I think we're on the same page now, its just about adding your graphic to the docs so that things are clear to end-users & a trivial renaming of the params

[kaichie]

Yes I think we are on the same page now. Sorry I was quite occupied in the last two weeks. I will jump back into this on the coming weekend!

[kaichie]

The core features have been updated based on the reviews and discussions above. Documentation and test cases are currently in progress, with more to follow later this week.

[SteveMacenski]

Great!

[AlexeyMerzlyakov]

Review in progress. More comments will arrive shortly

[mergify]

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

[kaichie]

@SteveMacenski @AlexeyMerzlyakov I have rewrite the VelocityPolygon class, using Polygon as it's base class and will be added as a new type -- velocity_polygon. All parameter reading and logic are now consolidated within the same file, which looks much cleaner. I have also updated the code based on the feedback and reviews.

[SteveMacenski]

@kaichie pull in main, any remaining linting failure are then things this PR introduces. Obviously, we need to address test coverage of this feature but its understandable that you haven't gotten there yet given the level of rewriting we've asked for. You should prioritize this now, no more large changes needed. I assume you tested this on the robot / simulation to make sure at a high level it works?

Very close now :-)

[kaichie]

Updated the reviewed comments and added unit tests.

I assume you tested this on the robot / simulation to make sure at a high level it works?

Re-tested the latest changes using the nav2 tb3 simulation:

cm_test.mp4

Next is to finish up the documentation.

[SteveMacenski]

• Docs: migration guide about changes, configuration guide with new params and brief description at top, tutorial show velocity polygons, and readme updates appropriate
• Docs: explain the open comments on the direction for omni, definitions of angles using the table you made, etc
• A nice video, for tutorial / readme

---

Then we can move on to the other PRs

• Approach model using dynamics for slowing down based on decelleration distances
• Approach model report in status message if the actual robot footprint (t = 0) is in collision causing a deadlock
• Option to use cmd_vel or odom for each velocity polygon
• allowing different limits for different sub-polygon

[SteveMacenski]

@doisyg please review

@kaichie your test coverage is being picked up now, that's all good. I have a few doc requests in the website PR though

[doisyg]

Finally tested and looks good to me! Great work!
Once thing that could be improved (probably as a separate PR) is an example of how to configure a velocity_polygon with action_type: "limit". When trying, I naively filled linear_limit and angular_limit in the sub-polygon parameter spaces (with no effect). Maybe worth considering allowing different limits for different sub-polygon ?

[SteveMacenski]

Added to @kaichie TODO list above: #3708 (comment)

@kaichie which are your next steps?