Initial draft of Controller Path Following Deviation Range API

[SteveMacenski]

This introduces an initial prototype of a Controller Deviation API so that a path following solution can have awareness of the intermediary goals from something like a fleet manager, VDA 5050, or navigate through poses whereas each intermediary goal / connecting edge has a strict allowable deviation range. We pass in the goal(s) on the path through the Action API into the controllers which can then be setup to use them as they see fit.

Please provide some feedback!

TODO

• Other applications useful? Pruning? Looping?
• Add other constraints to the follow path action? Feedback for Tracking State?
• naming - key_poses is a placeholder that I do not love
• use poses or index - should we use the actual poses (which then is less clear where on the path it is to constrain) or indices on the path (which is then requiring that the constrained poses are indeed on the path by definition)
• Should we add edge information or is the edge between goals sufficiently clear - it seems to me that the goals themselves are the nodes of the graph edges so adding in edge information is overly constrained
• Each algorithm to implement or ignore. Parameterize constraints deviation + how to handle.
  • MPPI/DWB validate at end and in the critics to influence trajectory selection within bounds. Bounds for both goal achievement and edge information. Have the deviation range be a ROS parameter.
  • RPP/Graceful validate and reject if out of range (default set range very high)
• Tests
• Documentation and announcement

[codecov]

Codecov Report

❌ Patch coverage is 87.50000% with 2 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ehavior_tree/plugins/action/follow_path_action.cpp	60.00%	2 Missing ⚠️

Files with missing lines	Coverage Δ
...ehavior_tree/plugins/action/follow_path_action.hpp	100.00% <100.00%> (ø)
...ller/include/nav2_controller/controller_server.hpp	100.00% <ø> (ø)
nav2_controller/src/controller_server.cpp	85.00% <100.00%> (ø)
nav2_core/include/nav2_core/controller.hpp	100.00% <ø> (ø)
...er/dwb_core/include/dwb_core/dwb_local_planner.hpp	100.00% <ø> (ø)
..._dwb_controller/dwb_core/src/dwb_local_planner.cpp	82.07% <100.00%> (ø)
...e/nav2_graceful_controller/graceful_controller.hpp	100.00% <ø> (ø)
...v2_graceful_controller/src/graceful_controller.cpp	89.69% <100.00%> (ø)
...roller/include/nav2_mppi_controller/controller.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/controller.cpp	93.87% <100.00%> (ø)
... and 5 more

... and 4 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[sampreets3]

Hello,

I really admire the fact that Nav2 is thinking about a tighter communication with Fleet Managers. I worked on implementing a VDA5050 adapter for our mobile robot based on ROS 2, I can provide some feedback. I'm also interested in hearing what the community thinks.

Should we add edge information or is the edge between goals sufficiently clear - it seems to me that the goals themselves are the nodes of the graph edges so adding in edge information is overly constrained

The edge information becomes necessary when you have edge actions associated with them --- in which case, it becomes imperative to let the VDA5050 Master controller know the current status of the robot. If you do not have edge actions, and you swear to never deal with edge actions, what you can do is simply pop the associated edge once you have traversed the node. To the MC, this will seem as if the traversal of the edge was instantaneous. This is not the cleanest way to deal with this, mind you.

That being said, I do not imagine that Nav2 will be dealing with a full integration of the VDA5050 standards, so the edge information for something like a path planner would be a bit extra. I'll try and explain myself a bit better. Consider a VDA5050 order where you go to Node A, then to Node B, pick up an item X, and drive to Node C, to drop it off. When the item is loaded, you must blink a light and let everybody know that you're transporting loads. In this case, the critical information necessary for Nav2 would simply be plan the trajectory Node A -> Node B -> Node C. I would imagine a higher level VDA5050 adapter node would be the one parsing this order and orchestrating everything on the robot.

The one reason I might be convinced in favour of passing in the edge info will be the possibility of launching edge actions (by calling appropriate behavior trees) while traversing the edge. If not, you leave it up to the higher level adapter node to figure out if they are on the edge by using the current pose of the robot/the sequenceId field in the order.

Looking forward to hearing thoughts on this. 🙂

[doisyg]

@BriceRenaudeau