Last fixes cpp.

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

Author: silanus23

nav2_util/src/path_utils.cpp

@@ -13,8 +13,11 @@
 // limitations under the License.
 
 #include "nav2_util/path_utils.hpp"
+
 #include <limits>
 #include <cmath>
+#include <stdexcept>
+
 #include "nav2_util/geometry_utils.hpp"
 namespace nav2_util
 {
@@ -29,26 +32,32 @@ PathSearchResult distance_from_path(
   result.closest_segment_index = start_index;
   result.distance = std::numeric_limits<double>::max();
 
-  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;
+  if (path.poses.empty()) {
     return result;
   }
 
-  if (start_index >= path.poses.size() - 1) {
+  if (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;
+    robot_pose.pose, path.poses.front().pose);
+    result.closest_segment_index = 0;
     return result;
   }
 
-  double distance_traversed = 0.0;
+  if (start_index >= path.poses.size()) {
+    throw std::invalid_argument(
+      "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 (path.header.frame_id != robot_pose.header.frame_id) {
+    throw std::invalid_argument(
+      "Invalid input, path frame (" + (path.header.frame_id) +
+      ") is not the same frame as robot frame (" + (robot_pose.header.frame_id) +
+      "). Use the same frame for both pose and path.");
+  }
+
+  double distance_traversed = 0.0;
   for (size_t i = start_index; i < path.poses.size() - 1; ++i) {
     if (distance_traversed > search_window_length) {
       break;

nav2_util/test/test_path_utils.cpp

@@ -29,7 +29,6 @@ geometry_msgs::msg::PoseStamped createPose(double x, double y)
   pose.pose.position.y = y;
   pose.pose.position.z = 0.0;
   pose.pose.orientation.w = 1.0;
-  pose.header.frame_id = "map";
   return pose;
 }
 
@@ -303,6 +302,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::distance_from_path(test_path, robot_pose, 1, 5.0);
-  EXPECT_NEAR(result.distance, 7.071, 0.01);
+  auto result = nav2_util::distance_from_path(test_path, robot_pose, 0, 5.0);
+  EXPECT_NEAR(result.distance, 5.0, 0.01);
 }