Add current path to GoalChecker interface

[ottojo]

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.

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]

@ottojo, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
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	85.44% <100.00%> (+0.03%)	⬆️

... and 1 file with indirect coverage changes

[SteveMacenski]

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

Agreed!

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 see you're marking this as a solution to the nav through poses ticket. I see this as a good API-change step needed to implement the goal checker specifics for that and I approve of this intermediate step. I think we do need to adjust the goal checker(s) or make a new one (probably just update the existing?) to fully close the ticket and actually use this API change.

So, I like where this is going - no objections and I look forward to the next step!

[SteveMacenski]

@ottojo following up here - have you had a chance to work on this?

[josephduchesne]

I'm also interested in this, and can potentially help out if it's blocked.

Not having this is a real headache for me, and I'd rather not duplicate @ottojo's patch solution (and make yet another proprietary goal checker that subscribes to the current path).

[SteveMacenski]

@josephduchesne I think we'd appreciate the help! This has been blocked / not updated in some time. Taking this and bringing it to the end would be great :-)

[ottojo]

@josephduchesne Please feel free to continue this, i don't currently have the time to continue this.

[SteveMacenski]

Closing in favor of #4789
Thanks @ottojo for getting this conversation started!