-
Notifications
You must be signed in to change notification settings - Fork 1.6k
Path distance feature #5387
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
Path distance feature #5387
Conversation
Codecov Report❌ Patch coverage is
... and 11 files with indirect coverage changes 🚀 New features to boost your workflow:
|
There was a problem hiding this 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!
aac9a09
to
7eb827a
Compare
@SteveMacenski sorry for late response. I made those changes you wanted. |
4361fe0
to
f419292
Compare
I changed util's cmake according to @mini-1235 's warning.
Adding clear costmap clearence is a future investment. |
There was a problem hiding this 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.
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. |
dab4b9c
to
9189594
Compare
@SteveMacenski Last local update: |
My bad on the delayed reviews. I think this is still relevant though as a next step:
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?
I'm not entirely sure what you mean by these. Can you expand?
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). |
There was a problem hiding this 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( |
There was a problem hiding this comment.
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
This pull request is in conflict. Could you fix it @silanus23? |
fc090a1
to
2926d3b
Compare
@silanus23, your PR has failed to build. Please check CI outputs and resolve issues. |
2926d3b
to
8c06085
Compare
@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 |
Sorry about that. That's because I have gone through a Git accident. I will post the proper version soon. |
@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 |
Signed-off-by: silanus23 <[email protected]>
@mini-1235 please let me know if all of your comments have been resolved |
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]>
There was a problem hiding this 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]>
There was a problem hiding this 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]>
Signed-off-by: silanus23 <[email protected]>
Signed-off-by: silanus23 <[email protected]>
There was a problem hiding this 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!
double current_distance_to_goal = 0.0; | ||
current_distance_to_goal = nav2_util::geometry_utils::euclidean_distance( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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_); |
There was a problem hiding this comment.
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, ¤t_path]() | ||
// Transform robot pose to path frame for path tracking calculations |
There was a problem hiding this comment.
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(), |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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( |
There was a problem hiding this comment.
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
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? |
Please fix the styling I pointed out. I'll look into it. It should have been automatically caught. |
Signed-off-by: silanus23 <[email protected]>
Basic Info
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:
backport-*
.