Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions by SteveMacenski · Pull Request #5207 · ros-navigation/navigation2

SteveMacenski · 2025-05-29T00:26:41Z

Resolves #5192

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

SteveMacenski · 2025-05-29T00:39:04Z

@codecov-ai-reviewer test

codecov-ai · 2025-05-29T00:39:10Z

On it! Codecov is generating unit tests for this PR.

Copilot

Pull Request Overview

This PR implements a prototype solution for Issue #5192 by refining the Reeds‐Shepp reverse expansion logic. Key changes include updating the AnalyticExpansion API to use a new structure with a “nodes” member and direction change tracking, modifying test assertions accordingly, and adding new launch parameters for simulation.

Reviewed Changes

Copilot reviewed 5 out of 5 changed files in this pull request and generated no comments.

Show a summary per file

File	Description
nav2_smac_planner/test/test_a_star.cpp	Updated the test assertion to reference the new “nodes” member in AnalyticExpansionNodes.
nav2_smac_planner/src/analytic_expansion.cpp	Refactored container accesses to use the “nodes” member, added the countDirectionChanges routine,
	and updated the refineAnalyticPath logic to account for directional changes in path scoring.
nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp	Updated the header to declare the new AnalyticExpansionNodes struct and countDirectionChanges method.
nav2_bringup/launch/tb3_simulation_launch.py and	Added two new simulation parameters (‘use_keepout_zones’ and ‘use_speed_zones’) to the launch scripts.
nav2_bringup/launch/tb3_loopback_simulation_launch.py

Comments suppressed due to low confidence (4)

nav2_smac_planner/test/test_a_star.cpp:150

The test assertion now correctly references the 'nodes' member of AnalyticExpansionNodes. Please confirm that all test expectations have been updated consistently across the codebase.

EXPECT_EQ(expected_nodes.nodes.size(), 0);

nav2_smac_planner/src/analytic_expansion.cpp:327

The update to check 'nodes.empty()' and the subsequent new condition comparing direction_changes improves the path selection logic. Ensure that these changes are thoroughly covered by tests to validate the intended behavior.

if (refined_analytic_nodes.nodes.empty()) {

nav2_bringup/launch/tb3_simulation_launch.py:198

The new launch parameter values are set as strings; confirm whether these should be booleans for consistency with other parameters and expected types.

'use_keepout_zones': 'False',

nav2_bringup/launch/tb3_loopback_simulation_launch.py:147

Similarly, verify that using string values for 'use_keepout_zones' (and 'use_speed_zones') aligns with the intended parameter type and processing in the launch configuration.

'use_keepout_zones': 'False',

mergify · 2025-05-29T00:45:49Z

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

codecov · 2025-05-29T02:20:00Z

Codecov Report

Attention: Patch coverage is 77.77778% with 8 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_smac_planner/src/analytic_expansion.cpp	80.00%	6 Missing ⚠️
...r/include/nav2_smac_planner/analytic_expansion.hpp	66.66%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...r/include/nav2_smac_planner/analytic_expansion.hpp	`81.81% <66.66%> (-18.19%)`	⬇️
nav2_smac_planner/src/analytic_expansion.cpp	`88.55% <80.00%> (-2.36%)`	⬇️

... and 5 files with indirect coverage changes

🚀 New features to boost your workflow:

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

wang-fire · 2025-05-30T16:04:07Z

Hi @SteveMacenski

I tested the nav2_smac_planner package using the Hybrid A* method, and the issue described in #5192 no longer appears.

But I noticed some very similar behavior near the end of the path (though it doesn't happen very frequently, it might happen once every 20 runs or so,). It looks like the robot is reversing, and the path segments are very short, usually concentrated in the last one or two nodes. I'm not sure if this is expected behavior, or if it might be caused by a high reverse penalty I set in the parameters (reverse_penalty: 300.0).

SteveMacenski · 2025-06-01T20:30:11Z

As I mentioned in #5192 (comment) this is expected and correct behavior. We want to be able to do this (that is the point of reeds shepp paths)! That is the shortest path to the goal.

The goal of this PR to make it less likely that we have those trivial reversal changes by not allowing refinements to create more of them -- and if a refinement can create less, we prioritize that. But sometimes that may legitimately be necessary as in the cases you show in the requested orientation.

…ansions (#5207) * prototype solving 5192 issue 2 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing unnecessary variable Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> (cherry picked from commit c32873d)

…ansions (#5207) (#5216) * prototype solving 5192 issue 2 * Removing unnecessary variable --------- (cherry picked from commit c32873d) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

…ll reverse expansions (ros-navigation#5207) * prototype solving 5192 issue 2 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing unnecessary variable Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

…erse expansions (#5207)" This reverts commit c32873d.

* Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207)" This reverts commit c32873d. * Revert "include bug fix for nav2_smac_planner (#5198)" This reverts commit 6a74ba6. * Revert "Feat/smac planner include orientation flexibility (#4127)" This reverts commit f5543c3.

…n#5207)

…ansions (#5207) * prototype solving 5192 issue 2 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing unnecessary variable Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207)" This reverts commit c32873d. * Revert "include bug fix for nav2_smac_planner (#5198)" This reverts commit 6a74ba6. * Revert "Feat/smac planner include orientation flexibility (#4127)" This reverts commit f5543c3.

* Add speed filter zones to depot and warehouse maps. (#5146) * Added keepout prefix to keepout zone related params. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Add namespace support for speed filter zones. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Added speed filter zones to depot and warehouse maps. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Added dedicated launch file for speed zone support. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Updated rviz2 config to include speed zone filter. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Preserve initial nature of costmap filters tests. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Enable namespace for the speed limit topic. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Update speed filter zones in depot and warehouse maps Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Readjust speed filter zones in depot and warehouse maps. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Enable namespace support for speed limit topic. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Reduced central speed zone in warehouse map. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> --------- Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * suppress rviz logging to warnings and above (#5163) * suppress rviz to warnings and above Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_bringup/launch/rviz_launch.py Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding Jazzy build job on Main PRs to automatically test Jazzy compatiblity long term (Kilted to come shortly) (#5164) * Adding jazzy testing job Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * on PR not push Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * move dir Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * more Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * moar Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * moar Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * more Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fin Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Prevent MPPI controller from resetting speed limits upon goal execution. (#5165) Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Backward docking without sensors (#5153) * Merge remote-tracking branch 'jakub/backward-docking-without-sensors' into docking_backward Signed-off-by: Jakubach <jakubach@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes and update controller test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add more tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Move backward_plugin param to plugin Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fixed rotateToDock and tests Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update readme Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added rotate_to_dock_timeout Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Use angular acceleration te acelerate / decelerate Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added mutex Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix exception Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Revert "Added mutex" This reverts commit 29ff010. Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Rename rotate_to_dock Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Rotate after undock and fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Log after rotation Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Jakubach <jakubach@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Speed up CI builds for released distros (#5168) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Save 2 minutes in Main-Jazzy build times to align with other CI job lengths (#5169) * Save 2 minutes in Main-Jazzy build times to align with other CI job lengths Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update build_main_against_distros.yml Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update build_main_against_distros.yml Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fixing docking server when already docked at the requeste ddock (#5171) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update mergify.yml Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix MPPI goal critic inversion (#5088) (#5105) * fix MPPI goal critic inversion (#5088) Signed-off-by: brayanpa <brayanspallares@gmail.com> * Support path inversion in all critics (#5088) Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix code style issues and formatting Signed-off-by: brayanpa <brayanspallares@gmail.com> * Remove trailing whitespaces Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix goalAngleCritic tests Signed-off-by: brayanpa <brayanspallares@gmail.com> * Normalize code formatting Signed-off-by: brayanpa <brayanspallares@gmail.com> * Add getLastPathPose test Signed-off-by: brayanpa <brayanspallares@gmail.com> * Abstract getCriticGoal in nav2_mppi critics Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix whitespace issues Signed-off-by: brayanpa <brayanspallares@gmail.com> * Fix getCriticGoal test Signed-off-by: brayanpa <brayanspallares@gmail.com> --------- Signed-off-by: brayanpa <brayanspallares@gmail.com> Signed-off-by: Brayan Pallares <brayanspallares@gmail.com> * Add stateful to regulated pure pursuit controller (#5167) * add_stateful_to_regulated_pure_pursuit_controller Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * fix naming Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * fix naming left over Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * resolve comments Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * typo Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * add unit test for both cases stateful and not stateful Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> --------- Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> * tf2 uses hpp headers in rolling (and is backported) (#5180) Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> * added config for laserscan in lb-sim (#5174) * added config for laserscan in lb-sim Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> * fixing ament_flake8 errors Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> * review: use_inf is default:true and added parameters on readme #4992 Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> * refactor: meaningfull value for scan_angle_increment Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> --------- Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> Co-authored-by: RamanaBotta <ramanab@bosonmotors.com> * Publish planned footprints after smoothing (#5155) * Publish planned footprints after smoothing Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "Publish planned footprints after smoothing" This reverts commit c9b349a. * Add smoothed footprints publishing Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix indentation for publisher creation in SmacPlannerHybrid and SmacPlannerLattice Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * address PR comments Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix build error Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixing deprecation warning (#5182) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing action server timeout duration after fixes to ROS 2, Reverts 3787 (#5183) * Removing action server timeout duration after fixes to ROS 2 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix build warning Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * action timeout in BT client edits error code and string (#5184) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Dynamic Parameters Only validating params that are part of the plugin (#5106) * Only validating params that are part of the plugin Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com> * review Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Refactoring type with param_type and name with param_name to get more consistency. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Check if plugin_name is part of param_name Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Check if param_name contains name_ Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Uncrustify Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Add check param name in dynamic parameter upate. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * fix Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Remove controller_frequency as dynamic parameter Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Revert "Merge remote-tracking branch 'origin/main' into FixNamespaceCheck" This reverts commit 7632e41, reversing changes made to 19afc9e. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Fix merge errors. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Add missing check to simple_goal_checker Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Handel param_name resolution for smac_planner_hybrid Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * fix typo Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * uncrustify Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * fix Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Revert "uncrustify" This reverts commit 43749c2. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * uncrustify Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Revert "uncrustify" This reverts commit e18f704. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Revert "fix" This reverts commit a0a7892. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Merge remote-tracking branch 'upstream/main' into FixNamespaceCheck Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * MPPI Check Namespace. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * fixing parameter_handler tests. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Fix optimizer Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Fix indentation Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * mppi param handler only execute post_callbacks if a param of mppi was updated. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Update nav2_rotation_shim_controller.hpp Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> * Review Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_mppi_controller/src/parameters_handler.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com> Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Increase Readability of testing. (#5190) Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Add value rewrites to RewrittenYaml (#5191) * Added context manager for safe file closing and prevent warnings. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Implement value_rewrites to ReWrittenYaml. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Use placeholders for costmap filters using the new value rewrite feature. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Enable system tests to use value rewrites. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Define remappings for costmap filters instead of inline substitution. Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> --------- Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Updates for Kilted Branch off (#5199) * Adding kilted build to main branch workflow Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Updating kilted for ci image build Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Bumping to 1.4.0 for kilted release Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing underlay workspace from Main (#5200) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding missing dep to loopback sim (#5204) * Adding missing dep Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * typo Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * updating fix Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding parameter warn_when_defaulting_parameters to control default parameter warnings (#5189) * Adding a parameter warn_when_defaulting_parameters to control default parameter warnings instead of using a flag Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> * Adding parameter strict_param_loading for optionally throwing an exception if parameter overrides are missing Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> * Using default false declaration instead of declare_or_get in param util Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> --------- Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> * Update mergify.yml Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * include bug fix for nav2_smac_planner (#5198) Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Revert "Removing underlay workspace from Main" (#5206) * Revert "Removing underlay workspace from Main (#5200)" This reverts commit 1278df7. * Update underlay.repos Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Option to Reduce Lethal to High-Cost Navigable To Get Out of Keepout Zones if Wandered In (#5187) * Adding toggle option of keepout zone Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Default off Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Join conditions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * spell check Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * copilot suggestions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update keepout_filter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> * Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207) * prototype solving 5192 issue 2 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Removing unnecessary variable Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Revert recent smac changes causing regressions (#5221) * Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207)" This reverts commit c32873d. * Revert "include bug fix for nav2_smac_planner (#5198)" This reverts commit 6a74ba6. * Revert "Feat/smac planner include orientation flexibility (#4127)" This reverts commit f5543c3. * Disable costmap filter zones from tb3 bringup (only Tb4 enabled) (#5223) * Update tb3_loopback_simulation_launch.py Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update tb3_simulation_launch.py Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Revert "Fix Ci from key signing (#5220)" (#5237) * Revert "Fix Ci from key signing (#5220)" This reverts the changes to the Dockerfile done in 1345c22. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Update Cache Version Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> --------- Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Updating readme table for kilted release (#5249) * updating readme table for kilted release Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Updating table lint Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * suggestion to time base and combine Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * typo Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Fixing builds for message filters API change while retaining Jazzy, Kilted, and Rolling support (#5251) * Update amcl_node.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Working for Kilted, Jazzy Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Route server corner smoothing (#5226) * added edge length method Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Added corner arc class Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * replaced double vectors with Coordinates, added methods to return start and end coordinates Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * using Coordinates, fixed direction of tangents Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added corner arc in header, added logger in protected variable Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * first pass of corner smoothing algorithm Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * reassigning next edge to have a different start, if a corner occurs before it Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * using unique pointer instead of raw pointers for new edges and nodes Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added smoothing parameter Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * made angle of interpolation a parameter Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * const for return methods, added flag for smoothing corners Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * moved getEdgeLength() into the Directional Edge struct Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * using float instead of double Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * smoothing radius is float, couple methods moved to protected Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * removed signed_angle_ as a member variable Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * removed unnecessary member variables Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * removed angle of interpolation and inferring it from path density and radius instead Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * consolidated corner arc into one header function Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * readded newline Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * changed corner arc to corner smoothing Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * replaced the use of edges with coordinates to generate smoothing arc, removed storage of nodes and edges Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * fixing cpplint Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting for headers Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * cpplinting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_route/src/path_converter.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixed divide by zeros and accessing empty route.edges Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * uncrustify linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * cpp linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * path converter linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * changed all doubles to floats Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added check for edges that are colinear to avoid divide by 0, fixed final edge interpolation Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * added doxygen for corner arc class Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added warning message if corner can't be smoothed Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added smooth_corners to the nav2 params file Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added smoothing flag and radius parameter to README.md' Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * typo in README Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * added testing for corner smoothing Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_route/include/nav2_route/corner_smoothing.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Conserve curvature with LIMIT action (#5255) * Conserve curvature with LIMIT action Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix format Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Parametrizing obstacle layer tf filter tolerance (#5261) Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> * Add namespace support for rviz costmap cost tool (#5268) Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> * Fix/smac planner orientation goals (#5235) * cherry pick Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * cherry pick 6a74ba6 Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * cherrpy pick Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * include x11 forwarding Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * kind of working version Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * cleanup Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * formatting Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * minor format change Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * change naming Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * minor changes Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * working with new changes Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Revert "Fix Ci from key signing (#5220)" (#5237) * Revert "Fix Ci from key signing (#5220)" This reverts the changes to the Dockerfile done in 1345c22. Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> * Update Cache Version Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> --------- Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Revert back Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * enable_groot_monitoring_ false (#5246) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Updating readme table for kilted release (#5249) * updating readme table for kilted release Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Updating table lint Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * suggestion to time base and combine Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * typo Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Fixing builds for message filters API change while retaining Jazzy, Kilted, and Rolling support (#5251) * Update amcl_node.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Working for Kilted, Jazzy Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update amcl_node.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * Change max_cost default to 254 (#5256) Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * linter Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * remove const Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * pass const pointer by value Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * pass const pointer by value Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> * remove unused param Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> --------- Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> * Increase cache Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixes for humble main compatibility Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Revert "Removing action server timeout duration after fixes to ROS 2, Reverts 3787 (#5183)" This reverts commit c9438b4. * fixing CI build Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * revert Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * done Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Jakubach <jakubach@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: brayanpa <brayanspallares@gmail.com> Signed-off-by: Brayan Pallares <brayanspallares@gmail.com> Signed-off-by: Jad haj mustafa <jhajmustafa@lely.com> Signed-off-by: Tim Clephas <tim.clephas@nobleo.nl> Signed-off-by: RamanaBotta <ramanab@bosonmotors.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Nils-ChristianIseke <nilsmailiseke@gmail.com> Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com> Signed-off-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: Maurice-1235 <mauricepurnawan@gmail.com> Co-authored-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com> Co-authored-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Brayan Pallares <brayanspallares@gmail.com> Co-authored-by: Jad Haj Mustafa <jado-91@hotmail.com> Co-authored-by: Tim Clephas <tim.clephas@nobleo.nl> Co-authored-by: Raman <52356167+RamanRobotics@users.noreply.github.com> Co-authored-by: RamanaBotta <ramanab@bosonmotors.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Co-authored-by: Marco Bassa <101661130+MarcoMatteoBassa@users.noreply.github.com> Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Co-authored-by: mini-1235 <mauricepurnawan@gmail.com>

* Revert "Prototype solving ros-navigation#5192 Issue 2: Reeds-Shepp reduce small reverse expansions (ros-navigation#5207)" This reverts commit c32873d. * Revert "include bug fix for nav2_smac_planner (ros-navigation#5198)" This reverts commit 6a74ba6. * Revert "Feat/smac planner include orientation flexibility (ros-navigation#4127)" This reverts commit f5543c3.

…n#5207)

* Revert "Prototype solving ros-navigation#5192 Issue 2: Reeds-Shepp reduce small reverse expansions (ros-navigation#5207)" This reverts commit c32873d. * Revert "include bug fix for nav2_smac_planner (ros-navigation#5198)" This reverts commit 6a74ba6. * Revert "Feat/smac planner include orientation flexibility (ros-navigation#4127)" This reverts commit f5543c3.

prototype solving 5192 issue 2

0669715

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

SteveMacenski mentioned this pull request May 29, 2025

nav2_smac_planner, path is not continuous #5192

Closed

Removing unnecessary variable

c040216

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

SteveMacenski requested a review from Copilot May 29, 2025 00:38

Copilot AI reviewed May 29, 2025

View reviewed changes

codecov-ai bot mentioned this pull request May 29, 2025

Add Tests for PR#5207 #5208

Closed

SteveMacenski added the backport-kilted label Jun 1, 2025

SteveMacenski merged commit c32873d into main Jun 1, 2025
13 of 15 checks passed

SteveMacenski deleted the 5192 branch June 1, 2025 20:31

mergify bot mentioned this pull request Jun 1, 2025

Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (backport #5207) #5216

Merged

SteveMacenski added a commit that referenced this pull request Jun 2, 2025

Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small rev…

7222287

…erse expansions (#5207)" This reverts commit c32873d.

SteveMacenski mentioned this pull request Jun 2, 2025

Revert recent smac changes causing regressions #5221

Merged

SteveMacenski added a commit that referenced this pull request Jun 2, 2025

Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small rev…

3b11d0f

…erse expansions (#5207)" This reverts commit c32873d.

SteveMacenski mentioned this pull request Jun 2, 2025

Smac revert changes kilted #5222

Merged

Jakub-husarz added a commit to MuLTechnologies/navigation2 that referenced this pull request Jun 3, 2025

smac_planner: update to the latest official code (up to ros-navigatio…

c076d57

…n#5207)

Jakub-husarz added a commit to MuLTechnologies/navigation2 that referenced this pull request Jul 9, 2025

smac_planner: update to the latest official code (up to ros-navigatio…

b01b316

…n#5207)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions#5207

Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions#5207
SteveMacenski merged 2 commits intomainfrom
5192

SteveMacenski commented May 29, 2025 •

edited

Loading

Uh oh!

SteveMacenski commented May 29, 2025 •

edited

Loading

Uh oh!

codecov-ai bot commented May 29, 2025

Uh oh!

Copilot AI left a comment

Uh oh!

mergify bot commented May 29, 2025

Uh oh!

codecov bot commented May 29, 2025 •

edited

Loading

Uh oh!

wang-fire commented May 30, 2025 •

edited

Loading

Uh oh!

SteveMacenski commented Jun 1, 2025 •

edited

Loading

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

SteveMacenski commented May 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented May 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

codecov-ai bot commented May 29, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

mergify bot commented May 29, 2025

Uh oh!

codecov bot commented May 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

wang-fire commented May 30, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Jun 1, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SteveMacenski commented May 29, 2025 •

edited

Loading

SteveMacenski commented May 29, 2025 •

edited

Loading

codecov bot commented May 29, 2025 •

edited

Loading

wang-fire commented May 30, 2025 •

edited

Loading

SteveMacenski commented Jun 1, 2025 •

edited

Loading