navigation2 icon indicating copy to clipboard operation
navigation2 copied to clipboard

Add current path to GoalChecker interface

Open ottojo opened this issue 6 months ago • 4 comments

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

  • [x] Extend GoalChecker::isGoalReached interface with argument for current path
  • [x] 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

ottojo avatar Aug 04 '24 11:08 ottojo