fix(nav2_collision_monitor): use velocity magnitude for holonomic checks by mach0312 · Pull Request #5954 · ros-navigation/navigation2

mach0312 · 2026-02-12T02:06:37Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5740
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Custom holonomic hardware platform
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Fixed an issue in VelocityPolygon where linear_min and linear_max checks were applied only to cmd_vel.x, ignoring lateral velocity for holonomic robots.
Updated isInRange logic to calculate the resultant velocity magnitude when holonomic is set to true, ensuring safety limits apply to the actual speed of motion in any direction.
This change enables defining speed-dependent safety polygons for omnidirectional movements (e.g., distinguishing between slow and fast lateral/diagonal sliding).
Preserved the existing behavior for non-holonomic robots (checking only cmd_vel.x) to ensure backward compatibility.

Description of documentation updates required from your changes

Need to update the VelocityPolygon documentation to clarify that when holonomic is set to true, linear_min and linear_max represent the resultant speed magnitude.
Consequently, these values must be non-negative (>= 0.0) for holonomic robots.
It should also be noted that for non-holonomic robots (holonomic: false), the behavior remains unchanged, and negative values are still used for backward motion.
I am currently working on these updates for docs.nav2.org and will submit a separate PR shortly.

Description of how this change was tested

Successfully built the package and verified that there are no compilation errors.
Updated velocity_polygons_test to reflect the logic change (checking velocity magnitude instead of just x-component for holonomic robots) and confirmed that all tests passed using colcon test.
Validated on a custom holonomic hardware platform:
- Confirmed that lateral and diagonal movements now correctly trigger the speed-dependent safety polygons based on the resultant velocity.
- Verified that non-holonomic behaviors remain unaffected

Forward/Backward movement
Screencast from 2026-02-09 12-11-16.webm

Sideways/Lateral movement
Screencast from 2026-02-09 12-08-13.webm

Future work that may be required in bullet points

Consider standardizing linear_min and linear_max to always represent velocity magnitude (speed) for all robot types in the future to ensure parameter consistency.
However, this would be a breaking change for existing non-holonomic configurations that rely on signed values (negative for backward motion).
Therefore, this PR preserves the legacy behavior (signed values) for holonomic: false to guarantee backward compatibility.

For Maintainers:

Check that any new parameters added are updated in docs.nav2.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
Should this be backported to current distributions? If so, tag with backport-*.

SteveMacenski

There appear to be some problems related to an older version of Nav2 than what Main requires. Please revert the use of nav2_utils

nav2_collision_monitor/src/velocity_polygon.cpp

mini-1235

Please also fix the linting and sign with DCO

nav2_collision_monitor/test/velocity_polygons_test.cpp

Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com>

Signed-off-by: Jaerak Son <sjr9017@naver.com>

… requirements Signed-off-by: Jaerak Son <sjr9017@naver.com>

…polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com>

SteveMacenski · 2026-02-12T22:27:57Z

@mach0312 can you pull in main / rebase to fix CI? We patched the error and I need to see coverage metrics.

How did your testing go? Were you able to show the bad behavior without and the good behavior with the patch?

codecov · 2026-02-13T00:22:33Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_collision_monitor/src/velocity_polygon.cpp	`96.55% <100.00%> (+0.32%)`	⬆️

... and 6 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

mach0312 · 2026-02-13T01:32:00Z

@SteveMacenski

Rebased on main to fix the CI issues.

Yes, I confirmed the bad behavior without the patch and the correct behavior with it.

1. Technical Explanation of the "Bad Behavior"

The core issue lies in how the isInRange function initializes its check.

The Problem: The boolean in_range is initialized strictly based on the X component (cmd_vel_in.x), regardless of whether the robot is holonomic or not.
Scenario: Consider a holonomic robot moving purely sideways with v_x = 0.0 and v_y = 0.5, targeting a velocity polygon defined for moving (e.g., linear_min = 0.1).
Initialization Check: The code evaluates cmd_vel_in.x >= sub_polygon.linear_min_. Since 0.0 >= 0.1 is False, in_range is initialized to false.
Holonomic Block Failure: Inside the if (holonomic_) block, the code performs a bitwise AND assignment (in_range &= ...). Since in_range started as false, it remains false regardless of the direction check.
Conclusion: As a result, the code incorrectly determines that the robot is "stopped" or out of range because it ignores the velocity magnitude contributed by the Y-axis. My patch fixes this by using std::hypot(x, y) to calculate the true speed magnitude.

2. Verification (Videos)

I performed a minimal test using a standalone collision_monitor node with a static scan point and manually published cmd_vel commands.

Note: The videos below are demos for verification purposes. I have also validated this patch on real hardware and confirmed that it works correctly during actual driving. (Please refer to the videos included in this PR.)

Before Patch (Bad Behavior)
In this video, I publish a pure lateral velocity (v_y = 0.5). As you can see, the polygon does not change and remains in the "stopped" or "slow" state because the X velocity is 0.0, causing the logic to fail.

Screencast.from.2026-02-13.10-08-13.webm

After Patch (Good Behavior)
With the patch applied, I publish the same lateral velocity (v_y = 0.5). The node now correctly calculates the magnitude using std::hypot, recognizes the speed, and switches to the appropriate extended polygon to cover the safety zone.

Screencast.from.2026-02-13.10-01-02.webm

SteveMacenski

Just waiting for docs update fixes :-)

…cks (ros-navigation#5954) * fix(nav2_collision_monitor): use velocity magnitude for holonomic checks Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com> * style: fix lint whitespace errors and typos Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage in velocity_polygons_test to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Add missing copyright header to nav2_collision_monitor/test/velocity_polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com> Signed-off-by: panav <panav@10xconstruction.com>

…cks (ros-navigation#5954) * fix(nav2_collision_monitor): use velocity magnitude for holonomic checks Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com> * style: fix lint whitespace errors and typos Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage in velocity_polygons_test to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Add missing copyright header to nav2_collision_monitor/test/velocity_polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com>

…cks (ros-navigation#5954) * fix(nav2_collision_monitor): use velocity magnitude for holonomic checks Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com> * style: fix lint whitespace errors and typos Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage in velocity_polygons_test to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Add missing copyright header to nav2_collision_monitor/test/velocity_polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com> Signed-off-by: panav <panav@10xconstruction.com>

…cks (ros-navigation#5954) * fix(nav2_collision_monitor): use velocity magnitude for holonomic checks Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com> * style: fix lint whitespace errors and typos Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage in velocity_polygons_test to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Add missing copyright header to nav2_collision_monitor/test/velocity_polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com> Signed-off-by: panav <panav@10xconstruction.com> Signed-off-by: Maurice <mauricepurnawan@gmail.com>

…cks (ros-navigation#5954) * fix(nav2_collision_monitor): use velocity magnitude for holonomic checks Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com> * style: fix lint whitespace errors and typos Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Revert nav2_util usage in velocity_polygons_test to match main branch requirements Signed-off-by: Jaerak Son <sjr9017@naver.com> * Add missing copyright header to nav2_collision_monitor/test/velocity_polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com> --------- Signed-off-by: Jaerak Son <sjr9017@naver.com>

mach0312 mentioned this pull request Feb 12, 2026

Feature Request: Support resultant linear speed for holonomic VelocityPolygon (enable lateral speed bands) #5740

Closed

SteveMacenski reviewed Feb 12, 2026

View reviewed changes

nav2_collision_monitor/src/velocity_polygon.cpp Outdated Show resolved Hide resolved

nav2_collision_monitor/src/velocity_polygon.cpp Show resolved Hide resolved

mach0312 force-pushed the collision_monitor branch from f34d158 to 96c298a Compare February 12, 2026 03:33

mini-1235 requested changes Feb 12, 2026

View reviewed changes

nav2_collision_monitor/test/velocity_polygons_test.cpp Show resolved Hide resolved

mach0312 added 5 commits February 12, 2026 14:46

fix(nav2_collision_monitor): use velocity magnitude for holonomic checks

0bd443e

Previously, VelocityPolygon only checked cmd_vel.x, ignoring lateral motion. This fix calculates the velocity vector magnitude when holonomic is set to true. Signed-off-by: Jaerak Son <sjr9017@naver.com>

style: fix lint whitespace errors and typos

06474f2

Signed-off-by: Jaerak Son <sjr9017@naver.com>

Revert nav2_util usage to match main branch requirements

2ea15ef

Signed-off-by: Jaerak Son <sjr9017@naver.com>

Revert nav2_util usage in velocity_polygons_test to match main branch…

73545f0

… requirements Signed-off-by: Jaerak Son <sjr9017@naver.com>

Add missing copyright header to nav2_collision_monitor/test/velocity_…

14eb2bf

…polygons_test.cpp Signed-off-by: Jaerak Son <sjr9017@naver.com>

mach0312 force-pushed the collision_monitor branch from ad0a7a1 to 14eb2bf Compare February 12, 2026 05:50

Merge branch 'ros-navigation:main' into collision_monitor

f42d08a

SteveMacenski approved these changes Feb 16, 2026

View reviewed changes

SteveMacenski linked an issue Feb 16, 2026 that may be closed by this pull request

Feature Request: Support resultant linear speed for holonomic VelocityPolygon (enable lateral speed bands) #5740

Closed

SteveMacenski merged commit 181570e into ros-navigation:main Feb 17, 2026
17 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

fix(nav2_collision_monitor): use velocity magnitude for holonomic checks#5954

fix(nav2_collision_monitor): use velocity magnitude for holonomic checks#5954
SteveMacenski merged 6 commits intoros-navigation:mainfrom
pitin-ev:collision_monitor

mach0312 commented Feb 12, 2026 •

edited

Loading

Uh oh!

SteveMacenski left a comment

Uh oh!

Uh oh!

Uh oh!

mini-1235 left a comment

Uh oh!

Uh oh!

SteveMacenski commented Feb 12, 2026 •

edited

Loading

Uh oh!

codecov bot commented Feb 13, 2026 •

edited

Loading

Uh oh!

mach0312 commented Feb 13, 2026

Uh oh!

SteveMacenski left a comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

mach0312 commented Feb 12, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

mini-1235 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

SteveMacenski commented Feb 12, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov bot commented Feb 13, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

mach0312 commented Feb 13, 2026

1. Technical Explanation of the "Bad Behavior"

2. Verification (Videos)

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

mach0312 commented Feb 12, 2026 •

edited

Loading

SteveMacenski commented Feb 12, 2026 •

edited

Loading

codecov bot commented Feb 13, 2026 •

edited

Loading