Add omni (SE2) analytic expansion to SmacPlannerLattice by ManMan88 · Pull Request #5965 · ros-navigation/navigation2

ManMan88 · 2026-02-17T16:34:05Z

Basic Info

Info
Ticket(s) this addresses	#2683, related: #5231, #4262
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Simulated rectangled shape holonomic AMR
Does this PR contain AI generated software?	Yes, not marked inline
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Added detection of motion_model: "omni" from the lattice metadata to select SE2StateSpace instead, which produces straight line paths with linear heading interpolation
Added MotionModel::OMNI enum value with string conversion support
Applied the same omni detection in the distance heuristic precomputation (reordered metadata loading to occur before state space selection)
Skip turning-radius refinement in refineAnalyticPath() for omni since SE2 paths don't depend on turning radius

Description of documentation updates required from your changes

New OMNI motion model enum value, maybe add a note in the SmacPlannerLattice configuration docs
No new parameters added, the omni motion model is auto-detected from the existing motion_model field in the lattice primitives file

Description of how this change was tested

Added 2 unit tests: test_omni_selects_se2_state_space and test_non_omni_selects_reeds_shepp
Manually tested with a holonomic AMR configuration and confirmed straight-line analytic expansions instead of arcs.

Future work that may be required in bullet points

Backport to jazzy (code structure differs slightly — precomputeDistanceHeuristic lives in node_lattice.cpp and analytic refinement is inline)

SteveMacenski · 2026-02-17T17:05:40Z

@ManMan88 can you please properly fill in the PR template?

SteveMacenski · 2026-02-17T17:12:09Z

Anything else we should update to optimize for omni platforms?

I think maybe this needs to be updated at least:

navigation2/nav2_smac_planner/src/node_lattice.cpp

Line 381 in 181570e

if (motion_model != MotionModel::STATE_LATTICE) {

& https://github.com/ManMan88/navigation2/blob/09d46dec9ae0ce662f7ea7fdbfcd78004e097b4d/nav2_smac_planner/src/analytic_expansion.cpp#L61-L62

Seems to me like this may not be functional (?)

codecov · 2026-02-17T17:23:27Z

Codecov Report

❌ Patch coverage is 95.23810% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/node_lattice.cpp	75.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...ac_planner/include/nav2_smac_planner/constants.hpp	`100.00% <ø> (ø)`
nav2_smac_planner/src/analytic_expansion.cpp	`89.01% <100.00%> (ø)`
nav2_smac_planner/src/distance_heuristic.cpp	`87.09% <100.00%> (+0.43%)`	⬆️
nav2_smac_planner/src/smac_planner_lattice.cpp	`91.02% <100.00%> (+1.08%)`	⬆️
nav2_smac_planner/src/node_lattice.cpp	`92.88% <75.00%> (+0.09%)`	⬆️

... and 13 files with indirect coverage changes

🚀 New features to boost your workflow:

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

ManMan88 · 2026-02-18T13:38:17Z

Sorry about the missing faulty PR format, I'll change it to the correct format based on the template.
Regarding your comment, you are correct, I added OMNI to the if statements of the motion model guards.

mini-1235

Could you also attach some screenshots / videos to help us better understand how the results differ after your changes?

SteveMacenski · 2026-02-18T19:35:29Z

I'm going to let @mini-1235 do the first few reviews to catch the big stuff but I wanted to thank you for taking this one. This is a much more elegant method than I had in mind and works much better.

I'd love for you to use some of those before/after in the docs.nav2.org migration guide to highlight this. This is a great contribution 💯

mini-1235

I left a few comments on items that need to be reverted. Basically, the _motion_model is hardcoded to STATE_LATTICE, so it can never be OMNI. But otherwise the changes looks good to me!

navigation2/nav2_smac_planner/src/smac_planner_lattice.cpp

Line 131 in 2b9e5f5

_motion_model = MotionModel::STATE_LATTICE;

Once this PR is merged, feel free to open a backport PR to jazzy

SteveMacenski · 2026-03-12T21:14:14Z

@ManMan88 any updates? I'd love to get this in!

ManMan88 · 2026-03-14T20:17:04Z

Sorry, I've been quite busy.
I'll take a look and make the required fixes this week.

ManMan88 · 2026-03-18T21:31:11Z

@mini-1235 I agree with your comments.
I updated thePR with a commit that reverted these changes.

Should I also add a similar fix for the jazzy branch?

SteveMacenski · 2026-03-19T17:55:15Z

Should I also add a similar fix for the jazzy branch?

Sure!

Also, can you add to the migration guide on docs.nav2.org this addition for omni support? Pick one of your before and after pictures for a side-by-side illustration I think would be useful.

Please sign off the commit with DCO. Check that failed job for how-to.

Finally - any other additions for optimal OMNI support or does this button that up?

ManMan88 · 2026-03-20T10:08:02Z

Finally - any other additions for optimal OMNI support or does this button that up?

I consulted opus 4.6, it found these issues:

Smoother uses non-holonomic mode for OMNI — SmootherParams::holonomic_ defaults to false and is never set to true for OMNI. This means Dubins-based boundary conditions are enforced on an omni robot's path during smoothing. Compare with smac_planner_2d.cpp:118 which explicitly sets params.holonomic_ = true. This is the most impactful gap.
allow_reverse_expansion can break OMNI — If a user sets allow_reverse_expansion=true with an OMNI lattice, the planner will append reversed-heading primitives with a reverse_penalty, which is meaningless for holonomic robots. Currently safe only because it defaults to false, but there's no guard preventing misconfiguration.

So I'll apply a fix for these as well.

mergify · 2026-03-20T10:56:26Z

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

When lattice file specifies motion_model: omni, use SE2StateSpace for analytic expansion instead of Dubins/Reeds-Shepp curves. This produces straight-line paths for holonomic robots instead of arc-based paths with unnecessary rotations. Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]>

Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]>

Set smoother to holonomic mode when lattice metadata indicates omni, avoiding incorrect Dubins boundary conditions on holonomic paths. Disable allow_reverse_expansion for omni robots with a warning, as the concept of reverse is meaningless for holonomic motion. Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]>

SteveMacenski

LGTM! Just the docs update I asked for which I think is good to highlight your work, and we can merge this!

Good catch on the smoother, I didn't think about that since this PR didn't touch it

ManMan88 · 2026-03-21T10:41:21Z

I updated the docs in this PR: ros-navigation/docs.nav2.org#891
Please let me know if this is sufficient.

SteveMacenski · 2026-03-23T17:04:10Z

I'll take a look now. Can you also add in a bit of the missing test coverage for the omni case in configuration / parameters to cover the lines added? https://app.codecov.io/gh/ros-navigation/navigation2/pull/5965?dropdown=coverage&src=pr&el=h1&utm_medium=referral&utm_source=github&utm_content=checks&utm_campaign=pr+comments&utm_term=ros-navigation

These seem relatively easy to add

mini-1235

LGTM with test added

SteveMacenski · 2026-03-30T21:07:36Z

@ManMan88 can you just address the small testing gaps so we can merge this? :-)

Signed-off-by: Ron Danon <[email protected]>

ManMan88 · 2026-03-30T23:12:53Z

I'll try to fix it again tomorrow

Signed-off-by: Ron Danon <[email protected]>

…on#5965) * Add omni analytic expansion support to SmacPlannerLattice When lattice file specifies motion_model: omni, use SE2StateSpace for analytic expansion instead of Dubins/Reeds-Shepp curves. This produces straight-line paths for holonomic robots instead of arc-based paths with unnecessary rotations. Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]> * Add OMNI to motion model guards in SmacPlannerLattice Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]> * Revert OMNI motion model guards that are unreachable Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]> * Fix smoother and reverse expansion for OMNI lattice robots Set smoother to holonomic mode when lattice metadata indicates omni, avoiding incorrect Dubins boundary conditions on holonomic paths. Disable allow_reverse_expansion for omni robots with a warning, as the concept of reverse is meaningless for holonomic motion. Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]> * Add OMNI test coverage for SmacPlannerLattice configure and reconfigure Signed-off-by: Ron Danon <[email protected]> * Fix OMNI test to directly exercise LatticeMotionTable::initMotionModel Signed-off-by: Ron Danon <[email protected]> --------- Signed-off-by: Ron Danon <[email protected]>

mini-1235 reviewed Feb 18, 2026

View reviewed changes

Comment thread nav2_smac_planner/src/node_lattice.cpp Outdated

mini-1235 requested changes Feb 20, 2026

View reviewed changes

SteveMacenski linked an issue Mar 12, 2026 that may be closed by this pull request

[Smac State Lattice] Support Omni Lateral Motions #2683

Closed

4 tasks

SteveMacenski reviewed Mar 19, 2026

View reviewed changes

Comment thread nav2_smac_planner/include/nav2_smac_planner/constants.hpp

Comment thread nav2_smac_planner/src/analytic_expansion.cpp

ManMan88 force-pushed the fix/omni-analytic-expansion branch from 767594b to 94c721b Compare March 20, 2026 11:04

ManMan88 added 4 commits March 20, 2026 13:07

Add OMNI to motion model guards in SmacPlannerLattice

815b285

Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]>

Revert OMNI motion model guards that are unreachable

cd013e7

Fixes ros-navigation#2683 Related: ros-navigation#5231, ros-navigation#4262 Signed-off-by: Ron Danon <[email protected]>

ManMan88 force-pushed the fix/omni-analytic-expansion branch from 94c721b to 46cb204 Compare March 20, 2026 11:08

SteveMacenski approved these changes Mar 20, 2026

View reviewed changes

ManMan88 mentioned this pull request Mar 21, 2026

Add OMNI analytic expansion migration guide entry for Kilted ros-navigation/docs.nav2.org#891

Merged

ManMan88 requested a review from mini-1235 March 21, 2026 11:19

mini-1235 approved these changes Mar 30, 2026

View reviewed changes

Add OMNI test coverage for SmacPlannerLattice configure and reconfigure

ae4518f

Signed-off-by: Ron Danon <[email protected]>

Fix OMNI test to directly exercise LatticeMotionTable::initMotionModel

57e7e14

Signed-off-by: Ron Danon <[email protected]>

ManMan88 force-pushed the fix/omni-analytic-expansion branch from 9d1c4e3 to 57e7e14 Compare March 31, 2026 10:26

SteveMacenski merged commit 795c30c into ros-navigation:main Mar 31, 2026
17 checks passed

Conversation

ManMan88 commented Feb 17, 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

Uh oh!

SteveMacenski commented Feb 17, 2026

Uh oh!

SteveMacenski commented Feb 17, 2026

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

ManMan88 commented Feb 18, 2026

Uh oh!

mini-1235 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

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

Uh oh!

Uh oh!

mini-1235 left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Mar 12, 2026

Uh oh!

ManMan88 commented Mar 14, 2026

Uh oh!

ManMan88 commented Mar 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Uh oh!

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

Uh oh!

Uh oh!

ManMan88 commented Mar 20, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mergify Bot commented Mar 20, 2026

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

ManMan88 commented Mar 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Mar 23, 2026

Uh oh!

mini-1235 left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Mar 30, 2026

Uh oh!

ManMan88 commented Mar 30, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

ManMan88 commented Feb 17, 2026 •

edited

Loading

codecov Bot commented Feb 17, 2026 •

edited

Loading

SteveMacenski commented Feb 18, 2026 •

edited

Loading

mini-1235 left a comment •

edited

Loading

ManMan88 commented Mar 18, 2026 •

edited

Loading

SteveMacenski commented Mar 19, 2026 •

edited

Loading

ManMan88 commented Mar 20, 2026 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

ManMan88 commented Mar 21, 2026 •

edited

Loading