Centralize path handler logic in controller server by mini-1235 · Pull Request #5446 · ros-navigation/navigation2

mini-1235 · 2025-08-12T14:03:32Z

This PR continues the work in #4789
Fixes #4757
Fixes #4304

This is a continuation of work started by @ottojo in draft PR #4593

I'll quote their introduction and, expand a bit afterwards, and update any fields that have changed.

The controller server uses a configurable goal checker to determine if the robot has completed its current path as defined by the global plan. Current goal checkers compare the goal (end of path) pose and twist to the current robot state, but that is not sufficient in all cases.
In some use cases, the path will visit the goal multiple times, and the goal might coincide with the starting position. It is still desired that the robot follows the entire path, instead of immediately ending navigation once the goal pose is reached.
This PR adds a parameter to the GoalChecker interface to inform the goal checker of the current path, which enables building more sophisticated goal checkers that (for example) take progress along the path into account.
We already use such a goal checker internally, which just subscribes to the global plan via the appropriate ROS topic, but i think this here is the cleaner solution (and also avoids race conditions of checking against an old path in the goal checker...)
I don't have a nice testing setup and a goal checker using this interface yet (other than our own robot and the mentioned goal checker), which is why I mark this as draft for now.

I've cherry-picked @ottojo 's commit on top of the latest main branch, then added a new basic goal checker PathCompleteGoalChecker, which is a subclass of SimpleGoalChecker, but has an additional parameter (path_length_tolerance, default=1), and checks that the current path has <= path_length_tolerance waypoints before allowing the normal SimpleGoalChecker completion logic to take its course.

The result is a trivial extension of SimpleGoalChecker that should be immune to the current_pose = goal_pose short-circuit problem/bug. Rather than try to create an elaborate unit test that navigates a course, I stuck to first principles to ensure that the new plugin behaves the same as SimpleGoalChecker with <= path_length_tolerance waypoints, and that it does not complete with > path_length_tolerance waypoints.

Basic Info

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

Description of contribution in a few bullet points

Extend GoalChecker::isGoalReached interface with argument for current path

Add this argument to the in-tree goal checker implementations

Add a goal checker which checks for path-completion

[] Define a test-scenario with global plan that returns to start pose

Description of documentation updates required from your changes

Migration guide for out-of-tree goal checkers

Future work that may be required in bullet points

Implementing more sophisticated goal checkers

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

mini-1235 · 2025-08-12T14:05:10Z

@SteveMacenski before I go too far, can you take a quick look to make sure I am on the right track? I have only pushed the changes in nav2_controller, graceful_controller and RPP

mergify · 2025-08-12T14:06:07Z

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

mini-1235 · 2025-08-12T18:38:05Z

I have spent some time reading into MPPI's path handler, I can see there are some improvements like #3659, #3443. Also, there is a related PR for RPP in #4763, maybe we need to check if these changes are applicable to all controllers? If yes, I guess it will be easier to move them in Controller Server

I also noticed that the DWB/MPPI need the path from planner to find its goal, should we pass this plan via the setplan api or a new argument in computevelocitycommands?

mamariomiamo · 2025-08-13T01:46:13Z

Hi, sorry to jump in here but just sharing my two cents:
Alternatively, is it possible to modify the end_pose_ to be the end of the pruned and transformed global path from the path_handler_? This way we do not need to add the additional PathCompleteGoalChecker since we will no longer be using the end of the global path for isGoalReached check. As the robot progresses along the global path, the pruned and transformed global path will eventually cover until the end of the global path and thus we will still be using the actual navigation goal as the final isGoalReached check.

mamariomiamo · 2025-08-13T03:42:48Z

I have spent some time reading into MPPI's path handler, I can see there are some improvements like #3659, #3443. Also, there is a related PR for RPP in #4763, maybe we need to check if these changes are applicable to all controllers? If yes, I guess it will be easier to move them in Controller Server

I also noticed that the DWB/MPPI need the path from planner to find its goal, should we pass this plan via the setplan api or a new argument in computevelocitycommands?

I think path_handler implementation from MPPI is more comprehensive as it also considers path up to inversion point. This would also be helpful when user uses Hybrid A* with reeds-shepp as the global planner.

mini-1235 · 2025-08-13T15:13:39Z

Hi, sorry to jump in here but just sharing my two cents: Alternatively, is it possible to modify the end_pose_ to be the end of the pruned and transformed global path from the path_handler_? This way we do not need to add the additional PathCompleteGoalChecker since we will no longer be using the end of the global path for isGoalReached check. As the robot progresses along the global path, the pruned and transformed global path will eventually cover until the end of the global path and thus we will still be using the actual navigation goal as the final isGoalReached check.

Thanks for sharing, I will try to implement this and compare with my current method

mergify · 2025-08-16T19:26:42Z

@mini-1235, 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

Sorry it took so long!

nav2_controller/include/nav2_controller/controller_server.hpp

nav2_controller/plugins/path_complete_goal_checker.cpp

nav2_controller/src/controller_server.cpp

SteveMacenski · 2025-08-28T23:27:09Z

I think path_handler implementation from MPPI is more comprehensive as it also considers path up to inversion point.

I agree with this general sentiment. I think it would be nice to review all the path handlers and make sure the key features of all are being respected in the server's implementation.

There's a number of CI failures that should also be looked at, but I'm sure you know :-)

mini-1235 · 2025-08-29T17:28:47Z

There's a number of CI failures that should also be looked at, but I'm sure you know :-)

Yes, I will continue this PR once #5496 and other future PRs targeting path handler are done

mergify · 2025-09-24T07:09:23Z

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

mini-1235 · 2025-09-25T14:35:45Z

@SteveMacenski I think you can start reviewing this when you have time, in the following days I plan to:

Finish DWB (Some logics are not handled yet)
Test Graceful/MPPI/RPP/DWB on different platform (Ackermann, Differential) with different params
Handle dynamic params in controller server
Fix & Add unit tests

Things to debate:

Alternatively, is it possible to modify the end_pose_ to be the end of the pruned and transformed global path from the path_handler_? This way we do not need to add the additional PathCompleteGoalChecker since we will no longer be using the end of the global path for isGoalReached check

I think this becomes problematic when enforce_path_inversion is set to true. In that case, the local goal ends up being the cusping point, and the robot may stop there once it satisfies the XY and yaw tolerances. Because of this, I believe we still need the global goal for the goal checker. But as @SteveMacenski suggested, we could add this directly into the simple goal checker, so by default the simple goal checker is checking both the global goal and local path length. Do you agree with this approach? @mamariomiamo

This PR has moved the following parameters from the plugin level to controller level:

transform_tolerance
prune_distance
max_robot_pose_search_dist
enforce_path_inversion
inversion_xy_tolerance
inversion_yaw_tolerance

Previously, the transform tolerance in the controller server was obtained from costmap->getTransformTolerance. I think it makes more sense for the server and its plugins to share the same tolerance, so I moved this to the controller level.

We have different pruned_plan_end for MPPI and RPP,

navigation2/nav2_controller/src/path_handler.cpp

Lines 134 to 146 in 3404f3a

    
           //TODO: For RPP and graceful we don't have prune distance, maybe we need to support two kinds of pruned_plan_end here 
        
           // by parameter? 
        
           if (1){ 
        
             pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( 
        
             closest_point, global_plan_up_to_inversion_.poses.end(), params_->prune_distance); 
        
           } 
        
           else{ 
        
             pruned_plan_end = std::find_if( 
        
             closest_point, global_plan_up_to_inversion_.poses.end(), 
        
             [&](const auto & global_plan_pose) { 
        
               return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; 
        
             }); 
        
           }

I am not sure how do we want to handle this, since only MPPI & DWB has the prune_distance parameter. Maybe adding a new parameter here will work?

Before merging this, I also need to:

Update nav2 tutorial with the new controller server API
Update docs, including migration guide, writing a new controller plugin, configuration guides

mergify · 2025-09-25T15:14:41Z

@mini-1235, 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 · 2025-09-29T20:42:21Z

OK will do! This is admittedly alot so this next round is going to be more high level.

... the local goal ends up being the cusping point, and the robot may stop there once it satisfies the XY and yaw tolerances ... we could add this directly into the simple goal checker, so by default the simple goal checker is checking both the global goal and local path length

Ah ok. That makes sense for the goal checker, though probably not the other elements right? When enforcing inversions is off, its the same either way since we just prune to the prune distance / costmap bounds. When is it enabled, then we prune to that which we want from the control algorithm's perspective, but not the goal checker.

Previously, the transform tolerance in the controller server was obtained from costmap->getTransformTolerance. I think it makes more sense for the server and its plugins to share the same tolerance, so I moved this to the controller level.

Why not have it still acquire it from costmap2D, just stored and used at the controller server level? I suppose I prefer less parameters to more, but I'm OK either way.

We have different pruned_plan_end for MPPI and RPP,

Do both - first after the integrated distance, as long as that distance is within the costmap bounds.

nav2_controller/src/parameter_handler.cpp

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

nav2_controller/include/nav2_controller/parameter_handler.hpp

nav2_core/include/nav2_core/controller.hpp

mini-1235 · 2025-10-01T15:10:13Z

Ah ok. That makes sense for the goal checker, though probably not the other elements right? When enforcing inversions is off, its the same either way since we just prune to the prune distance / costmap bounds. When is it enabled, then we prune to that which we want from the control algorithm's perspective, but not the goal checker.

Yes, when it is on, the distance to the local goal can become very small. That's why I think this local goal is not suitable to use in the goal checker

Why not have it still acquire it from costmap2D,

For the controller plugins as well?

Do both - first after the integrated distance, as long as that distance is within the costmap bounds.

Not sure if I understand this, can you elaborate?

SteveMacenski · 2025-10-01T15:56:43Z

For the controller plugins as well?

Isn't that in effect what is done today?

Not sure if I understand this, can you elaborate?

Basically combine the two:

navigation2/nav2_controller/src/path_handler.cpp

Lines 134 to 146 in 3404f3a

    
           //TODO: For RPP and graceful we don't have prune distance, maybe we need to support two kinds of pruned_plan_end here 
        
           // by parameter? 
        
           if (1){ 
        
             pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( 
        
             closest_point, global_plan_up_to_inversion_.poses.end(), params_->prune_distance); 
        
           } 
        
           else{ 
        
             pruned_plan_end = std::find_if( 
        
             closest_point, global_plan_up_to_inversion_.poses.end(), 
        
             [&](const auto & global_plan_pose) { 
        
               return euclidean_distance(global_plan_pose, robot_pose) > max_costmap_extent; 
        
             }); 
        
           }

. Find the first after the integrated distance. If its outside of the costmap bounds, then wind back until its within the costmap bounds. If its off the local costmap, then we cannot consider them for control since they're not validly known

SteveMacenski · 2025-10-01T16:40:18Z

A good suggestion I received today: It would be great to make the path handler itself a plugin so that other users can replace this implementation if they see fit. A nice side effect is it makes us have to think a bit more about the API interfaces for it to make sure its generally good for a broad range of purposes and include all the information for the APIs other implementations may want that we have access to.

Another would be possibly changing the pruning distance to be proportional to time instead, to prune the distance set out by that time by the maximum velocity. That way it wasn't something that needed to be tuned for changing velocity limits.

Finally, maybe it would be good to provide feedback about the current idx of the path we're on to use in the BT Navigator for computing ETA, path length remaining, and so forth. That way we handle the issue of path looping using the wrong index globally

mini-1235 · 2025-10-01T17:18:03Z

I will think about it and reply tomorrow

tonynajjar · 2025-12-17T21:27:27Z

nav2_bringup/params/nav2_params.yaml

      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
+      path_length_tolerance: 1.0
+    PathHandler:


@mini-1235 isn't max_robot_pose_search_dist missing?

I don't think we have this in the current config either, and in most cases I think costmap_size / 2 works well, so this isn't really something users need to tune. However, if you think it's necessary to add it here, I am happy to add it here and the docs PR

navigation2/nav2_bringup/params/nav2_params.yaml

Lines 118 to 142 in 4d9314d

FollowPath:

plugin: "nav2_mppi_controller::MPPIController"

time_steps: 56

model_dt: 0.05

batch_size: 2000

ax_max: 3.0

ax_min: -3.0

ay_max: 3.0

ay_min: -3.0

az_max: 3.5

vx_std: 0.2

vy_std: 0.2

wz_std: 0.4

vx_max: 0.5

vx_min: -0.35

vy_max: 0.5

wz_max: 1.9

iteration_count: 1

prune_distance: 1.7

transform_tolerance: 0.1

temperature: 0.3

gamma: 0.015

motion_model: "DiffDrive"

visualize: true

publish_optimal_trajectory: true

No strong opinion, I just thought that we expose in the yaml file everything that is a configurable parameter. But if the philosophy is rather to expose parameters that are likely to be tuned then I also understand that

@SteveMacenski what do you think?

I'd leave as-is. Half the costmap size is good for most users and if we set it here, then that would be out of date if a user changes the costmap size. I think its actually a better out of the box behavior by not including it for that reason. One less thing to tune.

nav2_controller/plugins/position_goal_checker.cpp

doisyg · 2025-12-18T17:08:25Z

Amazing work!

tonynajjar · 2025-12-19T13:57:59Z

@mini-1235 I found a bug: Try setting a param e.g. ros2 param set /controller_server FollowPath.vx_max 0.5 while the follow_path action is running, you should get a deadlock -> the param will never be set

Removing that lock fixes it but I know that has its drawbacks - I leave it to you to find best of both worlds

tonynajjar · 2025-12-19T16:13:54Z

I ran the PR against our tests suite

Please ignore that, something went wrong and I didn't test the right changes. I'll rectify and report back

Okay now I can say that aside from the deadlock issue, tests pass :)

mini-1235 · 2025-12-19T16:56:53Z

@mini-1235 I found a bug: Try setting a param e.g. ros2 param set /controller_server FollowPath.vx_max 0.5 while the follow_path action is running, you should get a deadlock -> the param will never be set

Removing that lock fixes it but I know that has its drawbacks - I leave it to you to find best of both worlds

Thanks for reporting @tonynajjar, I didn't notice that the original implementation was using try_lock. Looking back at the history, I found this comment #2833 (comment). For this PR, I will follow the same approach (try_lock and return if it fails), at least until there's interest from others to change this behavior

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

tonynajjar · 2025-12-19T17:13:07Z

@mini-1235 I just tried your latest commit quickly and I don't think they fix the deadlocks, did it work for you?

mini-1235 · 2025-12-19T17:19:27Z

@mini-1235 I just tried your latest commit quickly and I don't think they fix the deadlocks, did it work for you?

Yes, FollowPath.vx_max should not be locked because it satisfies the condition here

https://github.com/mini-1235/navigation2/blob/7a7ae73dfd70b54e61f4e30b7938ee1701a5b26d/nav2_controller/src/parameter_handler.cpp#L188-L190

I also tried setting failure_tolerance when FollowPath is running, I see the warning log here

https://github.com/mini-1235/navigation2/blob/7a7ae73dfd70b54e61f4e30b7938ee1701a5b26d/nav2_controller/src/parameter_handler.cpp#L192-L197

mini-1235 · 2025-12-19T17:39:56Z

One subtle issue I see is that the cli reports the parameter update as successful, even though it is actually rejected in our code. I am not sure if it makes sense to move the try_lock into validateParametersCallback so that the update is rejected when we are unable to lock, but that would make it hard for us to release the lock

tonynajjar · 2025-12-22T09:20:55Z

@mini-1235 I just tried your latest commit quickly and I don't think they fix the deadlocks, did it work for you?

Well, I don't know what changed but now it also works for me 😄 (typical Friday night testing)

Any estimate on when this can be merged? We would then proceed to get #5746 in

mini-1235 · 2025-12-22T10:12:09Z

Any estimate on when this can be merged? We would then proceed to get #5746 in

@SteveMacenski any comments on my last commit? Could you also retrigger the CI for me? I suspect this is just flaky tests and not related to the PR, since all tests pass before I pulled in the main branch. But if it fails again, I will take a look

tonynajjar · 2025-12-22T12:09:43Z

Could you also retrigger the CI for me?

Maybe you can just push a dummy commit to re-trigger?

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

mini-1235 · 2025-12-23T01:57:26Z

:( test is failing, I will check again tonight

edit: I can sometimes reproduce the failing test locally, this seems to be related to #3098 problem2, so I update the prune distance to 1.0

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

mini-1235 · 2025-12-30T11:15:33Z

Huge thanks to everybody involved in this PR! 🎉

* Add path goal checker plugin Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use local plan's last pose as end pose Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move path handler from plugin to controller level Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add exact path following in path handler and parameter handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move more parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rotation shim Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * RPP Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Graceful Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * MPPI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove goal from MPPI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update setPlan api Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix naming and doxygen Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove path complete goal checker Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert "Use local plan's last pose as end pose" This reverts commit 145b2d9. Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add prune distance Signed-off-by: Maurice <mauricepurnawan@gmail.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add dynamic parameter Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert unrelated changes Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * DWB Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert "Remove goal from MPPI" This reverts commit 5bc19f2da2649cd500ef11c374103f7de5a8116f. Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * New idea Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rename Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use new api Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Make path handler a controller server's plugin Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix dynamic parameters handling Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add mppi path handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add path length checking in position goal checker Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove controller frequency in dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix as suggested Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add api, fix dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add mppi logic in simple path handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Simplify test logic Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add output port in FollowPath Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Always return local plan in odom frame Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert tracking feedback output port Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Create post callback first Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add path length tolerance in dwb critic Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Format Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Activate dynamic parameters callback in Avtivate stage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests for dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Skip Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix as suggested Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update CI key for codecov Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Increase code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Increase code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move from controller_utils to path_utils Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update params and rviz Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update costmap update timeout Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Apply Tony's suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Feedback Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rename Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update rotation shim, path handler api, and constraint check Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Mark transformed global plan as const to prevent mutating it Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_util/include/nav2_util/path_utils.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move to cpp Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_util/src/path_utils.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix early termination bug Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Cleanup Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Check before logging Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use try lock in controller server Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Retrigger CI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Retrigger CI due to timeout Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update prune distance in keepout filter test Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice <mauricepurnawan@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Add path goal checker plugin Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use local plan's last pose as end pose Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move path handler from plugin to controller level Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add exact path following in path handler and parameter handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move more parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rotation shim Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * RPP Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Graceful Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * MPPI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove goal from MPPI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update setPlan api Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix naming and doxygen Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove path complete goal checker Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert "Use local plan's last pose as end pose" This reverts commit 145b2d9. Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add prune distance Signed-off-by: Maurice <mauricepurnawan@gmail.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add dynamic parameter Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert unrelated changes Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * DWB Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert "Remove goal from MPPI" This reverts commit 5bc19f2da2649cd500ef11c374103f7de5a8116f. Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * New idea Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rename Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use new api Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Make path handler a controller server's plugin Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix dynamic parameters handling Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add mppi path handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add path length checking in position goal checker Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Remove controller frequency in dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix as suggested Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add api, fix dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add mppi logic in simple path handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Simplify test logic Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add output port in FollowPath Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Always return local plan in odom frame Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert tracking feedback output port Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Create post callback first Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Minor fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add path length tolerance in dwb critic Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Format Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Activate dynamic parameters callback in Avtivate stage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests for dynamic parameters Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Skip Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix as suggested Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update CI key for codecov Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Increase code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Increase code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move from controller_utils to path_utils Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update params and rviz Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update costmap update timeout Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Apply Tony's suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Feedback Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Rename Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update rotation shim, path handler api, and constraint check Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Mark transformed global plan as const to prevent mutating it Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_util/include/nav2_util/path_utils.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/src/controller_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Update nav2_controller/plugins/feasible_path_handler.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Move to cpp Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Revert Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_util/src/path_utils.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix early termination bug Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add code coverage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Apply suggestions Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Cleanup Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Check before logging Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use try lock in controller server Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Retrigger CI Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Retrigger CI due to timeout Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update prune distance in keepout filter test Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice <mauricepurnawan@gmail.com> Signed-off-by: Maurice Alexander Purnawan <mauricepurnawan@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

SteveMacenski mentioned this pull request Aug 12, 2025

Goalchecker path argument and plugin #4789

Closed

11 tasks

mini-1235 mentioned this pull request Aug 15, 2025

Better management of RPP path pruning when cusp points are detected #4763

Closed

7 tasks

SteveMacenski requested changes Aug 28, 2025

View reviewed changes

This was referenced Sep 4, 2025

MPPI feedback for progress on global path #5498

Closed

Add exact path following in rpp for feasible plans #5496

Closed

SteveMacenski mentioned this pull request Sep 17, 2025

Path distance feature #5387

Merged

8 tasks

mini-1235 mentioned this pull request Sep 22, 2025

DWBLocalPlanner: NavToPose fails if prune_plan is disabled #3375

Closed

mini-1235 force-pushed the feat/goal_checker branch from 145b2d9 to ea45821 Compare September 22, 2025 17:22

mini-1235 mentioned this pull request Sep 24, 2025

Refactor common functions in smoother and planner #5537

Merged

8 tasks

mini-1235 changed the title ~~Add path goal checker plugin~~ Centralize path handler logic in controller server Sep 25, 2025

mini-1235 mentioned this pull request Sep 26, 2025

[Graceful Controller] Fix Incorrect Motion Target Heading output by controller #5530

Merged

8 tasks

SteveMacenski reviewed Sep 29, 2025

View reviewed changes

mini-1235 mentioned this pull request Oct 1, 2025

Update declares and gets to use new declare_or_get_param API #5299

Closed

tonynajjar reviewed Dec 17, 2025

View reviewed changes

nav2_controller/plugins/position_goal_checker.cpp Show resolved Hide resolved

Use try lock in controller server

7a7ae73

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

mini-1235 added 2 commits December 22, 2025 12:25

Retrigger CI

ac1cab4

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

Retrigger CI due to timeout

0be3208

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

Update prune distance in keepout filter test

5020c5d

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

tonynajjar requested a review from SteveMacenski December 29, 2025 09:48

tonynajjar approved these changes Dec 29, 2025

View reviewed changes

SteveMacenski merged commit f3406e2 into ros-navigation:main Dec 29, 2025
17 checks passed

mini-1235 deleted the feat/goal_checker branch December 30, 2025 11:15

mini-1235 mentioned this pull request Jan 8, 2026

Add tracking feedback in FollowPath output port #5854

Merged

8 tasks

Tacha-S mentioned this pull request Jan 13, 2026

Add footprint expansion to graceful controller #5779

Merged

8 tasks

mini-1235 mentioned this pull request Jan 14, 2026

Fix bt navigator feedback in looping path #5864

Merged

8 tasks

CihatAltiparmak mentioned this pull request Jan 27, 2026

Migrate to navigation2 PR : Centralize path handler logic in controller server CihatAltiparmak/frenet_ilqr_controller#67

Merged

mergify bot mentioned this pull request Jan 27, 2026

Migrate to navigation2 PR : Centralize path handler logic in controller server (backport #67) CihatAltiparmak/frenet_ilqr_controller#68

Closed

	FollowPath:
	plugin: "nav2_mppi_controller::MPPIController"
	time_steps: 56
	model_dt: 0.05
	batch_size: 2000
	ax_max: 3.0
	ax_min: -3.0
	ay_max: 3.0
	ay_min: -3.0
	az_max: 3.5
	vx_std: 0.2
	vy_std: 0.2
	wz_std: 0.4
	vx_max: 0.5
	vx_min: -0.35
	vy_max: 0.5
	wz_max: 1.9
	iteration_count: 1
	prune_distance: 1.7
	transform_tolerance: 0.1
	temperature: 0.3
	gamma: 0.015
	motion_model: "DiffDrive"
	visualize: true
	publish_optimal_trajectory: true

Conversation

mini-1235 commented Aug 12, 2025 • 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

Future work that may be required in bullet points

For Maintainers:

Uh oh!

mini-1235 commented Aug 12, 2025

Uh oh!

mergify bot commented Aug 12, 2025

Uh oh!

mini-1235 commented Aug 12, 2025

Uh oh!

mamariomiamo commented Aug 13, 2025

Uh oh!

mamariomiamo commented Aug 13, 2025

Uh oh!

mini-1235 commented Aug 13, 2025

Uh oh!

mergify bot commented Aug 16, 2025

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!

Uh oh!

Uh oh!

Uh oh!

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

Uh oh!

Uh oh!

mini-1235 commented Aug 29, 2025

Uh oh!

mergify bot commented Sep 24, 2025

Uh oh!

mini-1235 commented Sep 25, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

mergify bot commented Sep 25, 2025

Uh oh!

SteveMacenski commented Sep 29, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mini-1235 commented Oct 1, 2025

Uh oh!

SteveMacenski commented Oct 1, 2025

Uh oh!

SteveMacenski commented Oct 1, 2025

Uh oh!

mini-1235 commented Oct 1, 2025

Uh oh!

tonynajjar Dec 17, 2025 • 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!

mini-1235 Dec 18, 2025

Choose a reason for hiding this comment

Uh oh!

tonynajjar Dec 18, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 Dec 19, 2025

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Dec 20, 2025

Choose a reason for hiding this comment

Uh oh!

Uh oh!

doisyg commented Dec 18, 2025

Uh oh!

mini-1235 commented Aug 12, 2025 •

edited

Loading

SteveMacenski commented Aug 28, 2025 •

edited

Loading

mini-1235 commented Sep 25, 2025 •

edited

Loading

tonynajjar Dec 17, 2025 •

edited

Loading

tonynajjar commented Dec 19, 2025 •

edited

Loading

tonynajjar commented Dec 19, 2025 •

edited

Loading

mini-1235 commented Dec 23, 2025 •

edited

Loading