Goalchecker path argument and plugin

[josephduchesne]

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

[mergify]

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

[mergify]

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

A couple of followups:

• We should add in a new configuration guide page for this on docs.nav2.org. Also, a migration guide entry for the new feature!
• Is there a sensible reason why we shouldn't make this the default? I think we can update the nav2_params.yaml to always use this now & update the controller server's built-in default as well. This, I believe, is a globally better solution :-)

Generally though, this looks good to me

[josephduchesne]

A couple of followups:

• We should add in a new configuration guide page for this on docs.nav2.org. Also, a migration guide entry for the new feature!

Will do.

• Is there a sensible reason why we shouldn't make this the default? I think we can update the nav2_params.yaml to always use this now & update the controller server's built-in default as well. This, I believe, is a globally better solution :-)

I think this is a reasonable default. The only advantage that the existing built-in goal checkers have is that they require potentially far less math to check (which could be important on budget educational bots). I only made a new goal checker because that was what the original draft author had in his todo list :)

I'll update the default in the PR.

[josephduchesne]

I keep thinking that there is probably a way to use nav2_util::geometry_utils::first_after_integrated_distance rather than add a second short-circuiting checker to nav2_util::geometry_utils, but I don't think first_after_integrated_distance can be used to check if the remaining path exceeds a specific length without adding several other checks and calculations. It's so close!

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
..._controller/plugins/path_complete_goal_checker.hpp	100.00% <100.00%> (ø)
..._controller/plugins/path_complete_goal_checker.cpp	100.00% <100.00%> (ø)
nav2_controller/plugins/simple_goal_checker.cpp	100.00% <ø> (ø)
nav2_controller/plugins/stopped_goal_checker.cpp	100.00% <100.00%> (ø)
nav2_controller/src/controller_server.cpp	83.67% <100.00%> (+0.04%)	⬆️
nav2_util/include/nav2_util/geometry_utils.hpp	95.65% <100.00%> (+0.91%)	⬆️

... and 6 files with indirect coverage changes

[SteveMacenski]

I think the problem here is that if the path is not updated regularly (i.e. we plan once on start and only replan if there's a problem rather than at a fixed frequency), this would not work. We actually have logic in the Controller's path handlers to account for this by finding the robot's closest point on a path within a local window size of the path and storing that as our robot's "new starting point" to prune all points prior to. Then as we iterate, that is dynamically updated. As long as the search window is larger than the greatest distance a robot can move within the control frequency plus some margin, then it works on an iterative basis.

I agree this would work fine enough though for the case that we update the path on a regular frequency as long as the tolerance is sufficiently generous.

I think it would make sense to implement the same kind of logic here, no? If so, I think we should create a nav2_util function that does this path handling that we use here. Then, we can replace the copy-pasted version of that in all the controllers with a common util so they all behave the same! That would be a great architectural contribution

[josephduchesne]

I'll see what I can do :)

[SteveMacenski]

@josephduchesne can you button up these last few details so we can get this in? 😄

[mergify]

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

[mergify]

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

[mergify]

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

[josephduchesne]

I started to look into the path handler implementations to try to figure out a good approach for handling the path shortening aspect generically as you suggested. I'm not sure if I'll have time in the next few weeks to try to address that.

It hadn't occurred to me that this might be a problem since while I have been working with many different controllers, I'm doing so in the context of a Nav2 BT similar to the "NavigateWithReplanning" example, containing this bit that wraps Bathe controller/planner/path interaction.

<RateController hz="1.0">
  <ReactiveSequence>
    <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.45"/>
    <Sequence name="ComputeAndSmoothPath">
      <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
      <SmoothPath unsmoothed_path="{path}" smoothed_path="{path}" error_code_id="{smoother_error_code}"/>
    </Sequence>
  </ReactiveSequence>
</RateController>
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>

This means that aside from some slight quirks if I use very loosely defined paths + a very opinionated planner, the controller will suddenly cut corners or similar once a path goal is removed that was previously influencing intermediate steps in the planner or controller. That's not a huge problem, but interesting as an aside.

Back to the design at hand.

I think there's a tension around the "current_path_" use as I'm using it, how its used in controler_server.cpp computeAndPublishVelocity(), which also contains the "copy paste" you'd recommend removing (generating current_path, and perhaps ironically possibly having the exact same bug that I'm trying to fix here if the path contains loops, close zig-zags or self-intersections.

The issue with implementing that in the goal checker is that the goal checker's job currently doesn't include maintaining a stateful global plan, and updating it as the robot moves. That, I think, is the Controller's job, and the current_path_ passed in to the goal checker should be one that includes only present (maybe) and future path segments, not past ones.

We certainly could increase the scope of the goal checkers to be much more stateful to handle this. I'd rather move to trying to enforce a (soft) expectation that the controllers prune their paths.

Either way, I'm curious to hear your thoughts since I'm jumping back into reading this code after a month away from it, and far less familiar with Nav2's implementation on the best of days.

My hope would be either a clear decision that this PR requires a resolution to the global path is never updated issue, or agreement that we can split this off into its own enhancement request, and follow-up branch + PR down the road. I'm still interested in working on that either way, but I'm working on an agricultural robot that's got to be ready for the planting season, so my time is more limited the next few months.

[SteveMacenski]

I think there's a tension around the "current_path_" use as I'm using it, how its used in controler_server.cpp computeAndPublishVelocity(), which also contains the "copy paste" you'd recommend removing (generating current_path, and perhaps ironically possibly having the exact same bug that I'm trying to fix here if the path contains loops, close zig-zags or self-intersections.

See MPPI/RPP -- we don't search for the closest-point on the path in the full path scope but rather the closest point from a previous iteration's last point within a search window. That way, the window can be set to the maximum a robot can possibly move between iterations (plus margin) and remove the loop finding.

The issue with implementing that in the goal checker is that the goal checker's job currently doesn't include maintaining a stateful global plan, and updating it as the robot moves. That, I think, is the Controller's job, and the current_path_ passed in to the goal checker should be one that includes only present (maybe) and future path segments, not past ones.

I don't think that's always realistic, if you only plan at 1 hz but control at 20 hz, you are naturally going to make movement between them (let alone replanning every 5-10-20 seconds). I agree however that this is annoying logic to continuously reimplement, so it might be worth building a controller util that does this that we can deploy here, MPPI, RPP, etc more easily. The versions in MPPI/RPP are slightly different, however.

Perhaps an instance of the controller util, hosted by the controller server can be used by the goal checker? And/Or, the controller plugins are given both the global path and the pre-transformed and pruned path to use as their inputs, so we can remove some of the implementation details from each controller into centralized utilities that are computed in the server and then given to the plugins?

I think this would move the computation of the transformed / pruned plan into the controller when calling isGoalReached so that the goal checker itself is not maintaining a path / servicing its state changes. Now, its done by the controller and the input to this function is a pre-pruned pre-transformed plan. That plan could also be given to the controllers directly, if need be/desirable.

So I think the steps here:

• Create a new nav2_controller utility for path handling that performs this action based off of MPPI/RPP/Graceful/etc controllers
• Have this utility class composed into the Controller Server and compute the pruned path at some point
• Use this path for the goal checker path
• Consider if this utility can replace the versions (partially or fully) within RPP/MPPI/etc if we pass in this path to the controller plugins when we ask it to compute a trajectory. All is best, but I have a feeling ~1 of them might have unique specifics that make it want to handle its own from the raw path. If ~most, then we can pass it in both paths to decide how it wants to work with them

Does that make sense? 😄