Path distance feature

[silanus23]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5037
Primary OS tested on	Ubuntu
Robotic platform tested on	only unit tests
Does this PR contain AI generated software?	only numbers
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

Only path utils as descripted in the issue. It is finding nearest part in the path and current or waited position of robot. It interpolates between points cause the expected nearest point is between points path most of the time. There are 2 versions global version is reccomended and local version can sometimes fail.

Description of documentation updates required from your changes

At this stage none needed.

Description of how this change was tested

I wrote diffirent trajectories. AI help came in here I used it to calculate the numbers. I have tested the local search with clover leaf and retracting windows too but it failed. This was actually expected as the local search is optimized version. I don't think those failings come from a logical issue but the limitations of the approach behind it.

---

Future work that may be required in bullet points

Future side of the it is planned as you reccomend. Next one will be about creating a new msg type and publishing it from controller_server.

BTW thanks for patience about my last PR as this is my first open source contribution. I am still learning.

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
• Should this be backported to current distributions? If so, tag with backport-*.

[codecov]

Codecov Report

❌ Patch coverage is 95.83333% with 3 lines in your changes missing coverage. Please review.

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

Files with missing lines	Coverage Δ
...ller/include/nav2_controller/controller_server.hpp	100.00% <ø> (ø)
nav2_util/include/nav2_util/geometry_utils.hpp	97.22% <100.00%> (+0.99%)	⬆️
nav2_util/src/path_utils.cpp	100.00% <100.00%> (ø)
nav2_controller/src/controller_server.cpp	85.16% <90.32%> (+0.30%)	⬆️

... and 10 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]

Great - so after these changes this should be usable to find the closest point on the path. Can you identify the places in the codebase today we check for such things (controller plugins, progress checkers, BT navigators, etc) and how we might use this utility? That seems like the next logical step to me.

Then, expose that feedback to the controller server, a new bt node to use this as well. Once all that is in place, the controller critics for MPPI/DWB would round it all off!

[silanus23]

@SteveMacenski sorry for late response. I made those changes you wanted.
In my local branch I attempted to add the current implementation to the controller server. I use cross product to decide left or right of the track. Added that function cross products to geomtery utils. If negative left positive right kind of distinction I decided to use float32 (cause publishing string is not a prefered practice ).
Added the node to bt_navigator by b. tree xml's with the help of an addition rate controller. Now it's making system recreate path but it doesn't resolve the distance with path trajectory. So I think adding progress checker and critics will complete this step.
If you think this is the right approach I would like to continue with this way pushing it.

[silanus23]

I changed util's cmake according to @mini-1235 's warning.
@SteveMacenski as I said above I created a msg type that can provide things you wanted: current pose cross product and last segment index. Velocity can be added if you wish.
Cross product is to decide which side are we on of the track. A check for boundries of current_path_ could be added but added but I don't think it's needed cause they allready exist in util func.
About progress checkers: I couldn’t come up with a way to utilize the plugin style effectively, since we can publish data and track status from a BT node with more freedom. Please correct me if I’m wrong about the concepts.
In my plans checking the situation from a bt node and add that node to a tree like this way:

<RateController hz="3.0">
          <Fallback name="PathTrackingRecoveryPlanner">
            <IsWithinPathTrackingBounds max_tracking_error="2.0" />
            <Fallback name="TieredReplanning">
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
              <Sequence>
                <ClearEntireCostmap name="ClearGlobalCostmap-Tier2" service_name="global_costmap/clear_entirely_global_costmap"/>
                <RetryUntilSuccessful num_attempts="1">
                  <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
                </RetryUntilSuccessful>
              </Sequence>
            </Fallback>
          </Fallback>
        </RateController>

Adding clear costmap clearence is a future investment.
About critics I think going as proposed in the issue: averaging the deviation is the best way. would like to here your review.

[Copilot]

Pull Request Overview

This PR implements path distance functionality to calculate the minimum distance between a robot's current pose and a path. The feature enables tracking the robot's deviation from a planned path and provides real-time tracking error information.

• Adds path utility functions to compute distance from robot pose to path segments with optional windowed search
• Implements tracking error computation and publishing in the controller server
• Introduces a new TrackingError message type for real-time path deviation monitoring

Reviewed Changes

Copilot reviewed 10 out of 10 changed files in this pull request and generated 3 comments.

Show a summary per file

File	Description
nav2_util/include/nav2_util/path_utils.hpp	Defines PathSearchResult struct and distance_from_path function interface
nav2_util/src/path_utils.cpp	Implements core path distance calculation algorithm with windowed search
nav2_util/include/nav2_util/geometry_utils.hpp	Adds distance_to_segment and cross_product_2d utility functions
nav2_util/test/test_path_utils.cpp	Comprehensive test suite covering various path scenarios and edge cases
nav2_msgs/msg/TrackingError.msg	New message type for tracking error data
nav2_controller/include/nav2_controller/controller_server.hpp	Adds tracking error publishing capability to controller server
nav2_controller/src/controller_server.cpp	Integrates path distance calculation and publishes tracking error messages

---

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.
You can also share your feedback on Copilot code review for a chance to win a $100 gift card. Take the survey.}

[silanus23]

Hello @SteveMacenski First of all I am not trying to rush the proccess but to inform you about my current local progress so when you have time for my committed codes it will allign with your vision and we can move faster.  

About DWB Critics: I added the critics and a unit test. I have been able to create a case where that critic can create a diff here are the proof vids and parameters I used


.

About Costmap Layer: I get results in here but incosistent. and to be honest I am not really sure about my expectations in here. First path is not getting a corridor but the second is answering. In addition sometimes I can not get response too. But when I do get response I think I get a decent corridor around the path ahead of robot.

BT node: This started to work better with others giving less frequent fails and now able to recover faster.

MPPI critic: This one is a bit problematic as mppi is using diff kind of trajectory units. So I either have to convert it (which kills entire philosphy) or create an overloaded function that uses it's input types. I would like to hear your guidance if there is a better and efficient approach.

I missed 1 of your comments ( the 1 about doxygen of the util func) but I don't like too many tiny commits I will make it better in next commit.

I commit as you give feedback so I can be in synch with you and we can progress step by step.

I tried to find places to utilize added functions. I think maybe distance_to_segment can be used in someplace but it can optimize some places if used in place euclidean distance or optimization in planner side of the repo . I am really not sure about it. Maybe create an rviz2 plugin

Thanks for your guidance and time from now.

[silanus23]

@SteveMacenski Last local update:
I think I completed the code now. As you can see in the above, there are visual proofs, and I have added comments, logs, and unit test. In addition, I didn't make previous mistakes regarding lint, uncrust, and naming conventions. Of course, there can still be some reviews, but I believe this is working now.
Cheers.

[SteveMacenski]

My bad on the delayed reviews. I think this is still relevant though as a next step:

Great - so after these changes this should be usable to find the closest point on the path. Can you identify the places in the codebase today we check for such things (controller plugins, progress checkers, BT navigators, etc) and how we might use this utility? That seems like the next logical step to me.

Then, expose that feedback to the controller server, a new bt node to use this as well. Once all that is in place, the controller critics for MPPI/DWB would round it all off!

It sounds like you started on this, but I wanted to get a full list of possible uses before implementing to make sure we had the full list.

Rather than publishing as a topic, I think this would make sense to be a feedback element of the controller server that can be exposed to the behavior tree via the feedback for use. We can also have a BT node implemented that can compute it itself, I suppose. At that point, I think its worth going into the DWB/MPPI critics once we have something at the higher level -- though we could open the use of it as a separate PR so we can review that separately from the Controller Server / Behavior Tree integration points since those are nicely isolated from each other.

I think also publishing this as a topic isn't bad, so we can keep that too!

So, I think in summary, can you open a new PR called "Path Tracking Distance - Controller Plugins" with the DWB / MPPI work that we can review since you're far on that already?

---

Added the node to bt_navigator by b. tree xml's with the help of an addition rate controller. Now it's making system recreate path but it doesn't resolve the distance with path trajectory. So I think adding progress checker and critics will complete this step.

About progress checkers: I couldn’t come up with a way to utilize the plugin style effectively, since we can publish data and track status from a BT node with more freedom. Please correct me if I’m wrong about the concepts.

I'm not entirely sure what you mean by these. Can you expand?

---

About Costmap Layer: I get results in here but incosistent. and to be honest I am not really sure about my expectations in here. First path is not getting a corridor but the second is answering. In addition sometimes I can not get response too. But when I do get response I think I get a decent corridor around the path ahead of robot.

I think we should pause on this one at the moment and hone in on getting what you have started into the stack. The Controller critics for MPPI/DWB + the controller server / BT node is itself an accomplishment we can boast about 😉

But, if you wanted to look at it for a 3rd PR, basically: (a) create a new layer, (b) from a given path, compute the band of allowable area it can go from that path based on the max allowed deviation, (c) mark anything outside of this band as lethal space or very high space (parameterize the out-of-bounds cost).

[SteveMacenski]

Very good!

[SteveMacenski]

Note that there's some related work in #5446 (FYI @mini-1235) which moves the path hanlders into the controller server. This will prune the global path automatically and as such provide a path that is already in the controller server frame (odom) and prune so that the first point on the path is the starting point of the robot. That would astly simplify the logic in this function and above

[mergify]

This pull request is in conflict. Could you fix it @silanus23?

[mergify]

@silanus23, 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]

@silanus23 please don't force push - it actually makes it much harder for me to review since I can't see diffs of what changed. :-)

Take a look at CI - a bunch of tests failed and might be caused by this https://app.circleci.com/pipelines/github/ros-navigation/navigation2/16107/workflows/df9141b5-4e21-482e-a3e7-aaba01c88569/jobs/47067/tests

[silanus23]

Sorry about that. That's because I have gone through a Git accident. I will post the proper version soon.

[silanus23]

@SteveMacenski I think I handled your requests. Added tracking error and publishing it there. I added velocity and distance to goal in tracking_error.msg too and deleted them from path follow to not duplicate. In advance the sign of the tracking_error is now deciding as you say.

[SteveMacenski]

Are you using genAI for coding tools? I'm seeing unrelated formatting changes between updates... this seems really odd and making additional iteration cycles that wouldn't otherwise be necessary.

With these changes, I'm happy. @mini-1235 any final thoughts?

Next steps here are to use this for the BT nodes in a follow on PR!

[silanus23]

I started to use pre_commit normally I use colcon test and it would show the wrongs in styling but this time didn't show. Can it be the problem?

[SteveMacenski]

Please fix the styling I pointed out. I'll look into it. It should have been automatically caught.

[mini-1235]

Please ensure that all newly added functions are consistent with their doxygen documentation

[SteveMacenski]

@silanus23 merged! I think next steps are the BT node (which should be easy thankfully) and perhaps an example XML for how to use this and we're off to the races to use it in the plugins/costmap!