Path distance feature #5387

silanus23 · 2025-07-24T16:11:34Z

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

Signed-off-by: silanus23 <berkantali23@outlook.com>

codecov · 2025-07-24T17:39:55Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
nav2_util/include/nav2_util/geometry_utils.hpp	`96.36% <100.00%> (+1.24%)`	⬆️
nav2_util/src/path_utils.cpp	`100.00% <100.00%> (ø)`

... and 4 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 · 2025-07-25T00:40:38Z

nav2_util/include/nav2_util/geometry_utils.hpp

+ * @param target target point
+ * @return int
+ */
+inline double distanceToPoint(


I think you can just euclidean_distance which exists above for PoseStamped

Deleted it.

SteveMacenski · 2025-07-25T00:41:35Z

nav2_util/include/nav2_util/geometry_utils.hpp

+ * @param finish End point of target vector
+ * @return int
+ */
+inline double distanceToSegment(


Follow the naming conventions in this script for distance_to_segment please :-)

Sorry didn't realize first time

SteveMacenski · 2025-07-25T00:45:18Z

nav2_util/include/nav2_util/geometry_utils.hpp

+ * @return int
+ */
+inline double distanceToSegment(
+  const geometry_msgs::msg::PoseStamped & point,


I think we can make this point use geometry_msgs/Point which is just the PoseStamped msg.pose.position since this is a point value.

The start and end can be Pose so that we strip out the header. That can make this more portible to situations where we have geometry_msgs/Pose rather than PoseStamped for little change in the application code of pose to pose.pose.

Changed inputs to those.

SteveMacenski · 2025-07-25T00:46:10Z

nav2_util/include/nav2_util/geometry_utils.hpp

+  const geometry_msgs::msg::PoseStamped & start,
+  const geometry_msgs::msg::PoseStamped & end)
+{
+  const auto & p = point.pose.position;


I know this is a common math formulation, but can you add a link to the equation so that someone reading this layer understands where it came from? I typically like to do that for closed form equations to help the reader

Added a wikipedia link hope it's enough.

SteveMacenski · 2025-07-25T00:51:07Z

nav2_util/include/nav2_util/path_utils.hpp

+  size_t closest_segment_index;
+};
+
+PathSearchResult distanceFromPath(


distance_from_path

SteveMacenski · 2025-07-25T00:52:04Z

nav2_util/src/path_utils.cpp

+  return result;
+}
+
+PathSearchResult distanceFromPath(


I think you just want this one implementation, where the start_index is defaulted to 0 when not provided and search_window_length is set as numerical_limits<float>::max to remove duplication

Yeap exactly like you said :) sorry couldn't catch this in first.

SteveMacenski · 2025-07-25T00:52:34Z

nav2_util/src/path_utils.cpp

+{
+  PathSearchResult result;
+  result.closest_segment_index = start_index;
+  result.distance = std::numeric_limits<double>::infinity();


Remove all INF and replace with max in call cases. I don't like to intentionally inject INF and NAN into the system

I believe I handled this.

SteveMacenski · 2025-07-25T00:53:28Z

nav2_util/src/path_utils.cpp

+    return result;
+  }
+
+  if (search_window_length <= 0.0) {


I think this can throw - that's a totally invalid use

SteveMacenski · 2025-07-25T00:55:54Z

nav2_util/include/nav2_util/path_utils.hpp

+  double distance;
+  size_t closest_segment_index;
+};
+


ALl functions and objects need doxygen docs

SteveMacenski · 2025-07-25T00:56:19Z

nav2_util/src/path_utils.cpp

+  double min_distance = std::numeric_limits<double>::max();
+  size_t closest_segment = start_index;
+  size_t segments_checked = 0;
+  const size_t max_segments = static_cast<size_t>(search_window_length);


This is a double for a reason, its an actual distance. We should find the distance that each segment covers and add that to a running counter. Stop searching when the total euclidean distance along the path searched is greater than search_window_length

... this is making me a little suspicious that this was generated by AI - was it? I think this is the kind of thing you would know isn't the intent from a double input. Please make sure to review, in detail, before submitting PRs. That would also explain the code not generally following the styling of the rest of the files. Just a friendly warning, maintainers have limited time for reviews so try to make sure that you would put your name on something it outputs before sending to us to take a look.

I am so sorry this PR costed you. I used helper tools and not copy-pasting I was thinking I was reviewing them enough. But seems I was mistaken. Again and again I am gratefull for your patience and valuable time. I can guess how hard it is for you to deliver this kind of detailed review. I hope this time I met this repo's standarts. I will be even more strict in my future contributions. Thanks.

Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 added 6 commits July 21, 2025 07:16

geometry utils

38d7ba3

Signed-off-by: silanus23 <berkantali23@outlook.com>

fixed geometry utils adding 2d

78c0c32

Signed-off-by: silanus23 <berkantali23@outlook.com>

created path_utils

b899454

Signed-off-by: silanus23 <berkantali23@outlook.com>

added tests

c25fc31

Signed-off-by: silanus23 <berkantali23@outlook.com>

minor changes in tests

922e1c7

Signed-off-by: silanus23 <berkantali23@outlook.com>

lint issue

dad6a39

Signed-off-by: silanus23 <berkantali23@outlook.com>

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

SteveMacenski requested changes Jul 25, 2025

View reviewed changes

silanus23 added 2 commits July 28, 2025 23:46

fixed reviews

b394b16

Signed-off-by: silanus23 <berkantali23@outlook.com>

doxygen fix

aac9a09

Signed-off-by: silanus23 <berkantali23@outlook.com>

silanus23 requested a review from SteveMacenski July 29, 2025 09:42

Path distance feature #5387

Are you sure you want to change the base?

Path distance feature #5387

Conversation

silanus23 commented Jul 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

codecov bot commented Jul 24, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jul 25, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

silanus23 commented Jul 24, 2025 •

edited

Loading

codecov bot commented Jul 24, 2025 •

edited

Loading

SteveMacenski Jul 25, 2025 •

edited

Loading