Controller FollowPath feedback: Fix wrong frame in distance calculation, add more info to feedback

[adivardi]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu 24.04, Jazzy branch
Robotic platform tested on	gazebo simulation
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Fix a bug in controller's feedback computation: The robot pose was not transformed to the path pose, which gave wrong (and often changing) results
• Added closest_path_pose_idx and distance_to_path to the feedback
• very small fix in nav2_behavior_tree/nav2_tree_nodes.xml

Description of documentation updates required from your changes

none (?)

Description of how this change was tested

Tested on a Gazebo simulation of our robot, using Jazzy (same commits, just based on jazzy)

---

Future work that may be required in bullet points

Open question for discussion: Should the feedback from the FollowPath action be written into the Blackboard in the BT action node (follow_path_action)?

• see commit Write FollowPath Feedback to blackboard (reverted for now)
• Could also apply to any other action

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

[codecov]

Codecov Report

Attention: Patch coverage is 90.00000% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_controller/src/controller_server.cpp	90.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...ller/include/nav2_controller/controller_server.hpp	100.00% <ø> (ø)
nav2_controller/src/controller_server.cpp	83.54% <90.00%> (-0.09%)	⬇️

... and 2 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[SteveMacenski]

I think these should be removed or at least adjusted. The distance_to_path is not very accurate since it is the nearest path point, not the actual distance from the path. When using very dense paths, this might be reasonably accurate, but when the path markers are further away, then these measures are pretty poor.

How would these two values be used by client applications?

[adivardi]

true, but I think that's the best estimation we have without linearly interpolating the path, which introduces a different assumption. Shouldn't most paths be quite dense if they are coming from a nav2 planner?

For my use case: I am using them to monitor whether the robot has deviated the too much from the path and switch to another behavior. For example: while following a coverage path re-plan a detour path, or requesting user input if the deviation is way too big.

[SteveMacenski]

Shouldn't most paths be quite dense if they are coming from a nav2 planner?

Not always and/or promised. Right now, there aren't any assumptions that I'm aware of in Nav2 about the density of the path and there are custom user planners out there that are not as dense as those provided within Nav2.

Even so, if a costmap resolution is 5cm, some folks tracking error might only be 10 cm and the error from just the binning is already bordering the value.

I am using them to monitor whether the robot has deviated the too much from the path and switch to another behavior.

I think that might be better suited for a new method that takes in the path and the robot pose and compute that (with an optional argument of the closest path point; else find itself). That would actually be valuable contribution to nav2_util and then could be later used in the controller server to provide that feedback. I think the interpolation however is necessary.

That util could then also be used as a BT condition node itself to check if the path tracking accuracy has dropped. If so, then we go down a different branch. Another reason that this should be a util (and makes it unit testable)! This provides alot of flexibility in where it is computed and used

[adivardi]

removed the extra feedback.
I am not sure I have time to work on the extra util right now, should I open an issue so it is not forgotten?

[SteveMacenski]

Sorry for the delayed review, I've been having more thought-required PRs filed this week than I had time for thoughts to review them. This looks perfect to me, thanks for finding this bug!

Please do file file that ticket and please tag it with help wanted, and good first issue, its how alot of new contributors find projects and I think this is a good candidate for that!

[adivardi]

Opened.
I am not able to change label though.