Parallelize motion primitive generation and add trajectory_distinctness_ratio by mini-1235 · Pull Request #5866 · ros-navigation/navigation2

mini-1235 · 2026-01-15T12:16:18Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	closes #4991
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

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:

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-*.

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

mini-1235 · 2026-01-15T13:30:31Z

I have to fix #5837 first, I am having mypy issue on this PR :(

SteveMacenski

Feel free to merge once you're happy with mypy

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski · 2026-01-21T17:33:58Z

nav2_smac_planner/lattice_primitives/lattice_generator.py

+                            # Loop through the local trajectories with the same target heading
+                            for prev_pos, prev_len in trajectories_by_heading[target_heading]:
+                                dist = np.linalg.norm(target_point - prev_pos)
+                                if dist < self.trajectory_distinctness_ratio * min(current_length, prev_len):


Should this check happen in _is_minimal_trajectory?

Sure, like adding a new argument to _is_minimal_trajectory?

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

codecov · 2026-01-23T14:53:54Z

Codecov Report

✅ All modified and coverable lines are covered by tests.
see 30 files with indirect coverage changes

🚀 New features to boost your workflow:

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

SteveMacenski

Otherwise no notes!

nav2_smac_planner/lattice_primitives/README.md

...r/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

...r/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png

nav2_smac_planner/lattice_primitives/README.md

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

…ss_ratio (ros-navigation#5866) * Parallelize motion primitive generation Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Precommit Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add trajectory distinctness ratio Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move check to is_minimal_trajectory Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Drop previous length Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix test Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update readme Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * retrigger ci due to timeout Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_smac_planner/lattice_primitives/README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update readme Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add old_5cm_resolution Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove output.json Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

mini-1235 added 3 commits January 15, 2026 12:13

Parallelize motion primitive generation

11c4d3f

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Precommit

5b79a4f

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Lint

cf1cb4c

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

mini-1235 closed this Jan 15, 2026

mini-1235 reopened this Jan 15, 2026

mini-1235 force-pushed the feat/lattice_generator-1 branch from bf868af to cf1cb4c Compare January 15, 2026 13:28

SteveMacenski approved these changes Jan 15, 2026

View reviewed changes

Add trajectory distinctness ratio

8aa1c69

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski reviewed Jan 21, 2026

View reviewed changes

mini-1235 added 2 commits January 22, 2026 16:55

Merge remote-tracking branch 'origin/main' into feat/lattice_generator-1

566c04d

Move check to is_minimal_trajectory

646bd07

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

mini-1235 marked this pull request as draft January 22, 2026 18:44

mini-1235 added 3 commits January 23, 2026 11:08

Drop previous length

7f63816

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Fix test

3d8a4e7

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Update readme

e62f2c5

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

mini-1235 marked this pull request as ready for review January 23, 2026 11:49

mini-1235 changed the title ~~Parallelize motion primitive generation~~ Parallelize motion primitive generation and add trajectory_distinctness_ratio Jan 23, 2026

retrigger ci due to timeout

3339c20

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski reviewed Jan 23, 2026

View reviewed changes

nav2_smac_planner/lattice_primitives/README.md Outdated Show resolved Hide resolved

...r/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png Show resolved Hide resolved

Update nav2_smac_planner/lattice_primitives/README.md

789c178

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

SteveMacenski reviewed Jan 23, 2026

View reviewed changes

...r/lattice_primitives/sample_primitives/5cm_resolution/1m_turning_radius/all_trajectories.png Show resolved Hide resolved

nav2_smac_planner/lattice_primitives/README.md Show resolved Hide resolved

mini-1235 added 3 commits January 24, 2026 13:52

Update readme

70e6ff4

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Add old_5cm_resolution

5c01d11

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Remove output.json

b79a1bf

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

SteveMacenski approved these changes Jan 26, 2026

View reviewed changes

SteveMacenski merged commit 0d17014 into ros-navigation:main Jan 26, 2026
17 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Parallelize motion primitive generation and add trajectory_distinctness_ratio#5866

Parallelize motion primitive generation and add trajectory_distinctness_ratio#5866
SteveMacenski merged 14 commits intoros-navigation:mainfrom
mini-1235:feat/lattice_generator-1

mini-1235 commented Jan 15, 2026 •

edited

Loading

Uh oh!

mini-1235 commented Jan 15, 2026

Uh oh!

SteveMacenski left a comment

Uh oh!

SteveMacenski Jan 21, 2026

Uh oh!

mini-1235 Jan 21, 2026

Uh oh!

SteveMacenski Jan 21, 2026

Uh oh!

SteveMacenski Jan 21, 2026

Uh oh!

codecov bot commented Jan 23, 2026 •

edited

Loading

Uh oh!

SteveMacenski left a comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Conversation

mini-1235 commented Jan 15, 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!

mini-1235 commented Jan 15, 2026

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jan 21, 2026

Choose a reason for hiding this comment

Uh oh!

mini-1235 Jan 21, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jan 21, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jan 21, 2026

Choose a reason for hiding this comment

Uh oh!

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

Uh oh!

Codecov Report

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

mini-1235 commented Jan 15, 2026 •

edited

Loading

codecov bot commented Jan 23, 2026 •

edited

Loading