Centralize path handler logic in controller server

[mini-1235]

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]

@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]

@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]

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]

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]

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]

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]

@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!

[SteveMacenski]

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]

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]

@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]

@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:

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

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

3. 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]

@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]

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.

[mini-1235]

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]

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]

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]

I will think about it and reply tomorrow

[mini-1235]

#5446 (comment) -- do we need the full global path really? I think we discussed this elsewhere below as well that perhaps we didn't

I was thinking about one edge case:
If the forward sampling distance > prune distance, then the behavior may differ from what we currently have today.

#5446 (comment) -- breaking up a function into 2 methods that would be virtual that could be overridden in particular need situations to make the base path handler implementation the most reusable.

Leaving some of my thoughts here:
I totally agree with splitting it into two functions, since the original one is quite large and might not be reusable. The current implementation already split it into two functions, and also exposes the start/end pose and the transformed plan to be used in controller server. If someone proposes another plugin in the future, we can revisit this and see if there are any further improvements we can make

[mergify]

This pull request is in conflict. Could you fix it @mini-1235?

[SteveMacenski]

Add to other XMLs with the selector plugins like the default nav2pose?

[mini-1235]

I am a bit confused about which XMLs I should update here. Looking at the other XMLs, it seems this is the only behavior tree that uses GoalCheckerSelector and ProgressCheckerSelector, so I only added it here. Should I add it to all the XMLs as well?

[SteveMacenski]

There are others, but this is the other 'main' one I think you should add it to (also goal / progress checker) https://github.com/ros-navigation/navigation2/blob/main/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml#L11-L12

[SteveMacenski]

transformed_path.poses.reverse(input_path.poses.size();

[mini-1235]

I guess you mean reserve?

[SteveMacenski]

Yes :-) Sorry, that's my dyslexia speaking 😆

[SteveMacenski]

Please explain in a comment

[mini-1235]

If <= idx + 1, it means there cannot be a smaller index returned from the inversion/rotation check, otherwise a smaller index might exist, we just haven't checked it yet, I will add a comment

[SteveMacenski]

I wonder if some of this function can be simplified. For example, prev_valid seems like it could be replaced with simply idx > 1. When would we hit the line inversion_idx = std::min(inversion_idx, idx + 1); and idx+1 wouldn't be larger than the existing inversion_idx?

[mini-1235]

I agree that prev_valid can probably be replaced with idx >= 1, that was likely an oversight when I implemented it.

When would we hit the line inversion_idx = std::min(inversion_idx, idx + 1); and idx+1 wouldn't be larger than the existing inversion_idx?

Probably the very first time an inversion is detected, since inversion_idx is initialized to the path size

[SteveMacenski]

If the forward sampling distance > prune distance, then the behavior may differ from what we currently have today.

Log an error if this happens and set the prune distance to be the sampling distance in that case to make sure that's not possible.

[mini-1235]

If the forward sampling distance > prune distance, then the behavior may differ from what we currently have today.

Log an error if this happens and set the prune distance to be the sampling distance in that case to make sure that's not possible.

I don't think we could do that though. The rotation shim controller doesn't have access to the path_handler_ids, so it can't get these parameters directly. That means the only place we could do this logic would be in the controller server, but I don't think that's a clean or appropriate solution

[mini-1235]

When debugging, I encountered an issue with RPP (all default value):

Dec.10.2025.Video.mp4

The robot enters the shouldRotateToGoalHeading mode since the carrot pose has not yet been updated. Lowering the prune distance (default 2.0) solves the issue.

ps: I noticed that this didn't happen in the earlier version, maybe something wrong with my current implementation, I will double check again

edit: I understand the issue now, fixed in 2b02d7d

[SteveMacenski]

I don't think we could do that though. The rotation shim controller doesn't have access to the path_handler_ids, so it can't get these parameters directly. That means the only place we could do this logic would be in the controller server, but I don't think that's a clean or appropriate solution

OK! Docs in the configuration guide would do the trick. Practically speaking, I don't see this being an issue. In the Rotation Shim controller, if the path isn't at least the forward sampling distance long, we could log something

navigation2/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp

Lines 238 to 241 in 2b02d7d

	auto goal = current_path_.poses.back();
	goal.header.frame_id = current_path_.header.frame_id;
	goal.header.stamp = clock_->now();
	return goal;

. We know this would be the case on the approach to the goal, but if the current_path_.poses.back(); is not close to global_goal then we know that is not the situation we're in, so we could long once a warning. We'd just need to pass that into the method / use a class member and then make sure everything is in a consistent frame to compare pose distances.