fixed reviews

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

Author: silanus23

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -189,43 +189,32 @@ inline int find_next_matching_goal_in_waypoint_statuses(
 }
 
 /**
- * @brief Find the distance to a point
- * @param global_pose Robot's current or planned position
- * @param target target point
- * @return int
+ * @brief Find the shortest distance from a point to a line segment
+ *
+ * Uses the closed-form solution for the distance from a point to a segment in 2D.
+ * See: https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
+ *
+ * @param point The point to measure from
+ * @param start The start pose of the segment
+ * @param end The end pose of the segment
+ * @return double The shortest distance
  */
-inline double distanceToPoint(
-  const geometry_msgs::msg::PoseStamped & point1,
-  const geometry_msgs::msg::PoseStamped & point2)
+inline double distance_to_segment(
+  const geometry_msgs::msg::Point & point,
+  const geometry_msgs::msg::Pose & start,
+  const geometry_msgs::msg::Pose & end)
 {
-  const double dx = point1.pose.position.x - point2.pose.position.x;
-  const double dy = point1.pose.position.y - point2.pose.position.y;
-  return std::hypot(dx, dy);
-}
-
-/**
- * @brief Find the shortest distance to a vector
- * @param global_pose Robot's current or planned position
- * @param start Starting point of target vector
- * @param finish End point of target vector
- * @return int
- */
-inline double distanceToSegment(
-  const geometry_msgs::msg::PoseStamped & point,
-  const geometry_msgs::msg::PoseStamped & start,
-  const geometry_msgs::msg::PoseStamped & end)
-{
-  const auto & p = point.pose.position;
-  const auto & a = start.pose.position;
-  const auto & b = end.pose.position;
+  const auto & p = point;
+  const auto & a = start.position;
+  const auto & b = end.position;
 
   const double dx_seg = b.x - a.x;
   const double dy_seg = b.y - a.y;
 
   const double seg_len_sq = (dx_seg * dx_seg) + (dy_seg * dy_seg);
 
   if (seg_len_sq <= 1e-9) {
-    return distanceToPoint(point, start);
+    return euclidean_distance(point, a);
   }
 
   const double dot = ((p.x - a.x) * dx_seg) + ((p.y - a.y) * dy_seg);

nav2_util/include/nav2_util/path_utils.hpp

@@ -31,21 +31,27 @@
 namespace nav2_util
 {
 
+/**
+ * @brief Result of searching for the closest segment on a path.
+ */
 struct PathSearchResult
 {
+  /**
+   * @brief The minimum distance from the robot to the path.
+   */
   double distance;
+
+  /**
+   * @brief The index of the closest path segment.
+   */
   size_t closest_segment_index;
 };
 
-PathSearchResult distanceFromPath(
-  const nav_msgs::msg::Path & path,
-  const geometry_msgs::msg::PoseStamped & robot_pose);
-
-PathSearchResult distanceFromPath(
+PathSearchResult distance_from_path(
   const nav_msgs::msg::Path & path,
   const geometry_msgs::msg::PoseStamped & robot_pose,
-  const size_t start_index,
-  const double search_window_length);
+  const size_t start_index = 0,
+  const double search_window_length = std::numeric_limits<double>::max());
 
 }  // namespace nav2_util
 

nav2_util/src/path_utils.cpp

@@ -13,116 +13,62 @@
 // limitations under the License.
 
 #include "nav2_util/path_utils.hpp"
-
 #include <limits>
 #include <cmath>
-
 #include "nav2_util/geometry_utils.hpp"
-
 namespace nav2_util
 {
-PathSearchResult distanceFromPath(
-  const nav_msgs::msg::Path & path,
-  const geometry_msgs::msg::PoseStamped & robot_pose)
-{
-  PathSearchResult result;
-  result.closest_segment_index = 0;
-  result.distance = std::numeric_limits<double>::infinity();
-
-  if (path.poses.empty()) {
-    result.distance = std::numeric_limits<double>::infinity();
-    return result;
-  }
-
-  if (path.poses.size() == 1) {
-    result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front());
-    result.closest_segment_index = 0;
-    return result;
-  }
-
-  double min_distance = std::numeric_limits<double>::max();
-  size_t closest_segment = 0;
-
-  for (size_t i = 0; i < path.poses.size() - 1; ++i) {
-    const double current_distance = nav2_util::geometry_utils::distanceToSegment(
-      robot_pose,
-      path.poses[i],
-      path.poses[i + 1]);
-
-    if (current_distance < min_distance) {
-      min_distance = current_distance;
-      closest_segment = i;  // 🔥 Track closest segment in global search too
-    }
-  }
 
-  result.distance = min_distance;
-  result.closest_segment_index = closest_segment;
-  return result;
-}
-
-PathSearchResult distanceFromPath(
+PathSearchResult distance_from_path(
   const nav_msgs::msg::Path & path,
   const geometry_msgs::msg::PoseStamped & robot_pose,
   const size_t start_index,
   const double search_window_length)
 {
   PathSearchResult result;
   result.closest_segment_index = start_index;
-  result.distance = std::numeric_limits<double>::infinity();
+  result.distance = std::numeric_limits<double>::max();
 
-  if (path.poses.empty()) {
-    result.distance = std::numeric_limits<double>::infinity();
-    return result;
-  }
-
-  if (path.poses.size() == 1) {
-    result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.front());
+  if (path.poses.size() < 2) {
+    if (path.poses.empty()) {
+      return result;
+    }
+    result.distance = nav2_util::geometry_utils::euclidean_distance(
+      robot_pose.pose, path.poses.front().pose);
     result.closest_segment_index = 0;
     return result;
   }
 
-  if (start_index >= path.poses.size()) {
-    result.distance = std::numeric_limits<double>::infinity();
-    return result;
-  }
-
-  if (start_index == path.poses.size() - 1) {
-    result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose, path.poses.back());
-    result.closest_segment_index = start_index;
+  if (start_index >= path.poses.size() - 1) {
+    result.distance = nav2_util::geometry_utils::euclidean_distance(
+    robot_pose.pose,
+    path.poses.back().pose);
+    result.closest_segment_index = path.poses.size() - 1;
     return result;
   }
 
-  if (search_window_length <= 0.0) {
-    result.distance = nav2_util::geometry_utils::distanceToPoint(robot_pose,
-        path.poses[start_index]);
-    result.closest_segment_index = start_index;
-    return result;
-  }
-
-  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);
+  double distance_traversed = 0.0;
 
   for (size_t i = start_index; i < path.poses.size() - 1; ++i) {
-    const double current_segment_dist = nav2_util::geometry_utils::distanceToSegment(
-      robot_pose,
-      path.poses[i],
-      path.poses[i + 1]);
-
-    if (current_segment_dist < min_distance) {
-      min_distance = current_segment_dist;
-      closest_segment = i;
+    if (distance_traversed > search_window_length) {
+      break;
     }
 
-    segments_checked++;
-    if (segments_checked >= max_segments) {
-      break;
+    const double current_distance = geometry_utils::distance_to_segment(
+      robot_pose.pose.position,
+      path.poses[i].pose,
+      path.poses[i + 1].pose);
+
+    if (current_distance < result.distance) {
+      result.distance = current_distance;
+      result.closest_segment_index = i;
     }
+
+    distance_traversed += geometry_utils::euclidean_distance(
+      path.poses[i].pose,
+      path.poses[i + 1].pose);
   }
 
-  result.closest_segment_index = closest_segment;
-  result.distance = min_distance;
   return result;
 }
 

nav2_util/test/test_path_utils.cpp

@@ -18,6 +18,7 @@
 
 #include "gtest/gtest.h"
 #include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/path_utils.hpp"
 
@@ -168,20 +169,20 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths)
   auto robot_pose = createPose(5.0, 5.0);
   nav_msgs::msg::Path empty_path;
 
-  auto result = nav2_util::distanceFromPath(empty_path, robot_pose);
-  EXPECT_EQ(result.distance, std::numeric_limits<double>::infinity());
+  auto result = nav2_util::distance_from_path(empty_path, robot_pose);
+  EXPECT_EQ(result.distance, std::numeric_limits<double>::max());
 
   nav_msgs::msg::Path single_point_path;
   single_point_path.poses.push_back(createPose(0.0, 0.0));
-  result = nav2_util::distanceFromPath(single_point_path, robot_pose);
+  result = nav2_util::distance_from_path(single_point_path, robot_pose);
   EXPECT_NEAR(result.distance, 7.071, 0.01);
 }
 
 TEST_F(CuttingCornerTest, TrajectoryCutsCorner)
 {
   for (size_t i = 0; i < robot_trajectory.size(); ++i) {
     const auto & robot_pose = robot_trajectory[i];
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_NEAR(result.distance, expected_distances[i], 0.1);
   }
 }
@@ -191,15 +192,15 @@ TEST_F(RetracingPathTest, TrajectoryFollowsRetracingPath)
   const double expected_distance = 0.5;
 
   for (const auto & robot_pose : robot_trajectory) {
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_NEAR(result.distance, expected_distance, 1e-6);
   }
 }
 
 TEST_F(CloverleafPathTest, TrajectoryFollowsCloverleafLoop)
 {
   for (const auto & robot_pose : robot_trajectory) {
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_LT(result.distance, 0.25);
   }
 }
@@ -209,23 +210,23 @@ TEST_F(RetracingCircleTest, TrajectoryFollowsRetracingCircle)
   const double expected_distance = 0.2;
 
   for (const auto & robot_pose : robot_trajectory) {
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_NEAR(result.distance, expected_distance, 0.01);
   }
 }
 
 TEST_F(ZigZagPathTest, TrajectoryFollowsZigZagPath)
 {
   for (const auto & robot_pose : robot_trajectory) {
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_LT(result.distance, 1.0);
   }
 }
 
 TEST_F(HairpinTurnTest, TrajectoryFollowsHairpinTurn)
 {
   for (const auto & robot_pose : robot_trajectory) {
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose);
     EXPECT_LT(result.distance, 1.5);
   }
 }
@@ -239,7 +240,8 @@ TEST_F(CuttingCornerWindowedTest, WindowedSearch)
 
   for (size_t i = 0; i < robot_trajectory.size(); ++i) {
     const auto & robot_pose = robot_trajectory[i];
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index,
+      search_window);
     start_index = result.closest_segment_index;
     EXPECT_NEAR(result.distance, expected_distances[i], 0.15);
   }
@@ -255,7 +257,8 @@ TEST_F(RetracingPathWindowedTest, WindowedSearch)
 
   for (size_t i = 0; i < robot_trajectory.size(); ++i) {
     const auto & robot_pose = robot_trajectory[i];
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index,
+      search_window);
     start_index = result.closest_segment_index;
     EXPECT_NEAR(result.distance, expected_distance, 1e-6);
   }
@@ -270,7 +273,8 @@ TEST_F(ZigZagPathWindowedTest, WindowedSearch)
 
   for (size_t i = 0; i < robot_trajectory.size(); ++i) {
     const auto & robot_pose = robot_trajectory[i];
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index,
+      search_window);
     start_index = result.closest_segment_index;
     EXPECT_LT(result.distance, 1.0);
   }
@@ -285,7 +289,8 @@ TEST_F(HairpinTurnWindowedTest, WindowedSearch)
 
   for (size_t i = 0; i < robot_trajectory.size(); ++i) {
     const auto & robot_pose = robot_trajectory[i];
-    auto result = nav2_util::distanceFromPath(target_path, robot_pose, start_index, search_window);
+    auto result = nav2_util::distance_from_path(target_path, robot_pose, start_index,
+      search_window);
     start_index = result.closest_segment_index;
     EXPECT_LT(result.distance, 1.5);
   }
@@ -298,12 +303,6 @@ TEST(PathUtilsWindowedTest, EdgeCases)
   test_path.poses.push_back(createPose(0.0, 0.0));
   test_path.poses.push_back(createPose(10.0, 0.0));
 
-  auto result = nav2_util::distanceFromPath(test_path, robot_pose, 1, 5.0);
+  auto result = nav2_util::distance_from_path(test_path, robot_pose, 1, 5.0);
   EXPECT_NEAR(result.distance, 7.071, 0.01);
-
-  result = nav2_util::distanceFromPath(test_path, robot_pose, 0, 0.0);
-  EXPECT_NEAR(result.distance, 7.071, 0.01);
-
-  result = nav2_util::distanceFromPath(test_path, robot_pose, 10, 5.0);
-  EXPECT_EQ(result.distance, std::numeric_limits<double>::infinity());
 }