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]

Remove the extra 2 lines :-)

[SteveMacenski]

Why remove? I think we can add another so that we can manipulate both the path length here and the dynamic parameters there as well

[josephduchesne]

Good point, that's simpler. Will do.

[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 :)