nav2_smac_planner: handle corner case where start and goal are on the same cell #4793

DylanDeCoeyer-Quimesis · 2024-12-12T10:26:16Z

This case was already properly handled in the smac_planner_2d (since PR #2488), but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case.

Basic Info

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

Description of contribution in a few bullet points

Description of documentation updates required from your changes

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

codecov · 2024-12-12T11:38:54Z

Codecov Report

Attention: Patch coverage is 90.00000% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/smac_planner_2d.cpp	50.00%	1 Missing ⚠️
nav2_smac_planner/src/smac_planner_hybrid.cpp	93.33%	1 Missing ⚠️
nav2_smac_planner/src/smac_planner_lattice.cpp	92.30%	1 Missing ⚠️

Files with missing lines	Coverage Δ
nav2_smac_planner/src/smac_planner_2d.cpp	`93.20% <50.00%> (-0.43%)`	⬇️
nav2_smac_planner/src/smac_planner_hybrid.cpp	`86.85% <93.33%> (+0.08%)`	⬆️
nav2_smac_planner/src/smac_planner_lattice.cpp	`86.49% <92.30%> (+0.11%)`	⬆️

... and 3 files with indirect coverage changes

… same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue ros-navigation#4792. Signed-off-by: Dylan De Coeyer <[email protected]>

SteveMacenski

Otherwise, LGTM. Make sure to do these same comments in the State Lattice planner & add into the unit tests a plan where the start and goal are the same bin to exercise the change in unit testing

nav2_smac_planner/src/smac_planner_hybrid.cpp

Signed-off-by: Dylan De Coeyer <[email protected]>

…he same cell Signed-off-by: Dylan De Coeyer <[email protected]>

mergify · 2024-12-13T11:55:40Z

@DylanDeCoeyer-Quimesis, 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

Otherwise, LGTM. Test just needs to actually test the result of the exercised bit

nav2_smac_planner/test/test_smac_2d.cpp

mergify · 2024-12-16T09:20:31Z

@DylanDeCoeyer-Quimesis, 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).

Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <[email protected]>

SteveMacenski

Great work, thanks!

… same cell (ros-navigation#4793) * nav2_smac_planner: handle corner case where start and goal are on the same cell This case was already properly handled in the smac_planner_2d, but it was still leading to an A* backtrace failure in the smac_planner_hybrid and smac_planner_lattice. Let's harmonize the handling of this case. This commit fixes issue ros-navigation#4792. Signed-off-by: Dylan De Coeyer <[email protected]> * nav2_smac_planner: use goal orientation when path is made of one point Signed-off-by: Dylan De Coeyer <[email protected]> * nav2_smac_planner: publish raw path also when start and goal are on the same cell Signed-off-by: Dylan De Coeyer <[email protected]> * nav2_smac_planner: add corner case to unit tests Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <[email protected]> --------- Signed-off-by: Dylan De Coeyer <[email protected]>

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch 5 times, most recently from 5030af6 to 2f13bf1 Compare December 12, 2024 11:15

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch from 2f13bf1 to a912059 Compare December 12, 2024 11:55

SteveMacenski requested changes Dec 13, 2024

View reviewed changes

nav2_smac_planner/src/smac_planner_hybrid.cpp Outdated Show resolved Hide resolved

nav2_smac_planner/src/smac_planner_hybrid.cpp Show resolved Hide resolved

SteveMacenski linked an issue Dec 13, 2024 that may be closed by this pull request

SmacPlanner backtrace failure when goal is close to the start pose #4792

Closed

DylanDeCoeyer-Quimesis added 2 commits December 13, 2024 12:45

nav2_smac_planner: use goal orientation when path is made of one point

cc54623

Signed-off-by: Dylan De Coeyer <[email protected]>

nav2_smac_planner: publish raw path also when start and goal are on t…

76eda26

…he same cell Signed-off-by: Dylan De Coeyer <[email protected]>

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch from f820898 to 09eec6a Compare December 13, 2024 11:46

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch from 09eec6a to 728054f Compare December 13, 2024 14:45

SteveMacenski requested changes Dec 13, 2024

View reviewed changes

nav2_smac_planner/test/test_smac_2d.cpp Outdated Show resolved Hide resolved

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch 2 times, most recently from 5b44e9e to eba884f Compare December 16, 2024 09:17

nav2_smac_planner: add corner case to unit tests

81ee425

Add a plan where the start and goal are placed on the same cell. Signed-off-by: Dylan De Coeyer <[email protected]>

DylanDeCoeyer-Quimesis force-pushed the fix_smac_planner_backtrace_failure branch from eba884f to 81ee425 Compare December 16, 2024 09:31

SteveMacenski approved these changes Dec 17, 2024

View reviewed changes

SteveMacenski merged commit 27ba98a into ros-navigation:main Dec 17, 2024
11 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

nav2_smac_planner: handle corner case where start and goal are on the same cell #4793

nav2_smac_planner: handle corner case where start and goal are on the same cell #4793

DylanDeCoeyer-Quimesis commented Dec 12, 2024 •

edited

Loading

codecov bot commented Dec 12, 2024 •

edited

Loading

SteveMacenski left a comment

mergify bot commented Dec 13, 2024

SteveMacenski left a comment

mergify bot commented Dec 16, 2024

SteveMacenski left a comment

nav2_smac_planner: handle corner case where start and goal are on the same cell #4793

nav2_smac_planner: handle corner case where start and goal are on the same cell #4793

Conversation

DylanDeCoeyer-Quimesis commented Dec 12, 2024 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

codecov bot commented Dec 12, 2024 • edited Loading

Codecov Report

SteveMacenski left a comment

Choose a reason for hiding this comment

mergify bot commented Dec 13, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

mergify bot commented Dec 16, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

DylanDeCoeyer-Quimesis commented Dec 12, 2024 •

edited

Loading

codecov bot commented Dec 12, 2024 •

edited

Loading