Skip to content

Conversation

silanus23
Copy link

@silanus23 silanus23 commented Jul 24, 2025


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

@silanus23 silanus23 marked this pull request as ready for review July 24, 2025 17:08
Copy link

codecov bot commented Jul 24, 2025

Codecov Report

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

Files with missing lines Patch % Lines
nav2_controller/src/controller_server.cpp 90.62% 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.62%> (+0.30%) ⬆️

... and 11 files with indirect coverage changes

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@silanus23 silanus23 requested a review from SteveMacenski July 29, 2025 09:42
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

@Copilot Copilot AI review requested due to automatic review settings August 11, 2025 23:55
@silanus23 silanus23 force-pushed the path_distance_feature branch from aac9a09 to 7eb827a Compare August 11, 2025 23:55
Copilot

This comment was marked as outdated.

@silanus23
Copy link
Author

silanus23 commented Aug 11, 2025

@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 silanus23 force-pushed the path_distance_feature branch from 4361fe0 to f419292 Compare August 12, 2025 00:39
@silanus23
Copy link
Author

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.

@silanus23 silanus23 requested a review from Copilot August 15, 2025 13:38
Copy link
Contributor

@Copilot Copilot AI left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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
Copy link
Author

silanus23 commented Aug 20, 2025

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
with_path_hug-ezgif com-video-to-gif-converter (1)
without_path_hug-ezgif com-video-to-gif-converter (1)
.

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 silanus23 force-pushed the path_distance_feature branch from dab4b9c to 9189594 Compare September 9, 2025 21:58
@silanus23
Copy link
Author

costmap_visual

@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
Copy link
Member

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

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Very good!

const double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
robot_pose, end_pose_in_robot_frame);

const auto path_search_result = nav2_util::distance_from_path(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Contributor

mergify bot commented Sep 18, 2025

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

@silanus23 silanus23 force-pushed the path_distance_feature branch 2 times, most recently from fc090a1 to 2926d3b Compare September 18, 2025 19:21
Copy link
Contributor

mergify bot commented Sep 18, 2025

@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).

@silanus23 silanus23 force-pushed the path_distance_feature branch from 2926d3b to 8c06085 Compare September 19, 2025 16:50
@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 19, 2025

@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
Copy link
Author

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

@silanus23
Copy link
Author

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

Signed-off-by: silanus23 <[email protected]>
@SteveMacenski
Copy link
Member

@mini-1235 please let me know if all of your comments have been resolved

Copy link
Contributor

@mini-1235 mini-1235 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please remove the two comments here, and I will approve :)

Signed-off-by: silanus23 <[email protected]>
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There's some easy test coverage missing https://app.codecov.io/gh/ros-navigation/navigation2/pull/5387?dropdown=coverage&src=pr&el=h1&utm_medium=referral&utm_source=github&utm_content=checks&utm_campaign=pr+comments&utm_term=ros-navigation

if (path.poses.size() == 1) {
+     result.distance = nav2_util::geometry_utils::euclidean_distance(
+       robot_pose, path.poses.front().pose);
+     result.closest_segment_index = 0;
+     return result;
+   }

+   if (start_index >= path.poses.size()) {
+     throw std::runtime_error(
+       "Invalid operation: requested start index (" + std::to_string(start_index) +
+       ") is greater than or equal to path size (" + std::to_string(path.poses.size()) +
+       "). Application is not properly managing state.");
+   }

If you add those to the unit tests, we should be good to go!

Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

Comment on lines 687 to 688
double current_distance_to_goal = 0.0;
current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
double current_distance_to_goal = 0.0;
current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
double current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(

if (current_path_.poses.size() >= 2) {
double current_distance_to_goal = 0.0;
current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
pose, transformed_end_pose_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks like 4 spaces not 2? I don't know why CI isn't picking up on these linter errors right now


nav_msgs::msg::Path & current_path = current_path_;
auto find_closest_pose_idx = [&robot_pose_in_path_frame, &current_path]()
// Transform robot pose to path frame for path tracking calculations
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ditto here, why are there 4 spaces?

// Transform robot pose to path frame for path tracking calculations
geometry_msgs::msg::PoseStamped robot_pose_in_path_frame;
if (!nav2_util::transformPoseInTargetFrame(
pose, robot_pose_in_path_frame, *costmap_ros_->getTfBuffer(),
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ditto here - did you do some reformatting? I don't think this was an issue in previous reviews

const auto path_search_result = nav2_util::distance_from_path(
current_path_, robot_pose_in_path_frame.pose, start_index_, search_window_);

// Create tracking error message
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here too ... weird

nav2_util::geometry_utils::calculate_path_length(current_path_, start_index_);
start_index_ = path_search_result.closest_segment_index;

// Update current tracking error and publish
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ditto

// Use the current robot pose's timestamp for the transformation
end_pose_.header.stamp = pose.header.stamp;

nav2_util::transformPoseInTargetFrame(
Copy link
Member

@SteveMacenski SteveMacenski Oct 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Last functional change: We need this in an if statement as well to throw in case of error like the other one

@silanus23
Copy link
Author

silanus23 commented Oct 9, 2025

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
Copy link
Member

SteveMacenski commented Oct 9, 2025

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants