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 90.32258% with 6 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_controller/src/controller_server.cpp	86.95%	3 Missing ⚠️
nav2_util/src/path_utils.cpp	85.00%	3 Missing ⚠️

Files with missing lines	Coverage Δ
...ller/include/nav2_controller/controller_server.hpp	100.00% <ø> (ø)
nav2_util/include/nav2_util/geometry_utils.hpp	96.66% <100.00%> (+1.54%)	⬆️
nav2_controller/src/controller_server.cpp	84.94% <86.95%> (+0.11%)	⬆️
nav2_util/src/path_utils.cpp	85.00% <85.00%> (ø)

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

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!

[SteveMacenski]

Why is empty in the size < 2 condition? It is probably better to break it into 2 independent conditions for clarity. Check if empty --> return, then check if size == 1 to return the first point. I prefer flattened code over nested node as a matter of readability.

[SteveMacenski]

Change robot_pose from PoseStamped to Pose. Add in the doxygen its expected that the robot pose is already transformed into the path's frame

[SteveMacenski]

Or, check that the path frame == robot pose frame. If not, throw exception

[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.}

[Copilot]

[nitpick] Extra space after return keyword should be removed for consistency with coding standards.

Suggested change

	return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
	return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);

[Copilot]

[nitpick] Multiple statements on single lines reduce readability. Consider formatting each loop body on separate lines.

Suggested change

	for (int i = 0; i <= 10; ++i) {
	for (int i = 0; i <= 10; ++i) {
	target_path.poses.push_back(createPoseStamped(i, 0.0));
	}
	for (int i = 9; i >= 0; --i) {
	target_path.poses.push_back(createPoseStamped(i, 0.0));
	}
	for (int i = 0; i <= 10; ++i) {

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