Frame check fix

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

Author: silanus23

nav2_util/include/nav2_util/path_utils.hpp

@@ -44,14 +44,14 @@ struct PathSearchResult
  * It returns the minimum distance found and the index of the closest segment.
  *
  * @param path The path to search (sequence of poses).
- * @param robot_pose The robot's current pose.
+ * @param robot_pose The robot's current pose in pose form.
  * @param start_index The index in the path to start searching from.
  * @param search_window_length The maximum length (in meters) to search along the path.
  * @return PathSearchResult Struct containing the minimum distance and the index of the closest segment.
  */
 PathSearchResult distance_from_path(
   const nav_msgs::msg::Path & path,
-  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Pose & robot_pose,
   const size_t start_index = 0,
   const double search_window_length = std::numeric_limits<double>::max());
 

nav2_util/src/path_utils.cpp

@@ -24,7 +24,7 @@ namespace nav2_util
 
 PathSearchResult distance_from_path(
   const nav_msgs::msg::Path & path,
-  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Pose & robot_pose,
   const size_t start_index,
   const double search_window_length)
 {
@@ -38,7 +38,7 @@ PathSearchResult distance_from_path(
 
   if (path.poses.size() == 1) {
     result.distance = nav2_util::geometry_utils::euclidean_distance(
-    robot_pose.pose, path.poses.front().pose);
+    robot_pose, path.poses.front().pose);
     result.closest_segment_index = 0;
     return result;
   }
@@ -50,21 +50,14 @@ PathSearchResult distance_from_path(
       "). 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;
     }
 
     const double current_distance = geometry_utils::distance_to_segment(
-      robot_pose.pose.position,
+      robot_pose.position,
       path.poses[i].pose,
       path.poses[i + 1].pose);
 
@@ -74,8 +67,8 @@ PathSearchResult distance_from_path(
     }
 
     distance_traversed += geometry_utils::euclidean_distance(
-      path.poses[i].pose,
-      path.poses[i + 1].pose);
+      path.poses[i],
+      path.poses[i + 1]);
   }
 
   return result;

nav2_util/test/test_path_utils.cpp

@@ -22,7 +22,7 @@
 #include "nav_msgs/msg/path.hpp"
 #include "nav2_util/path_utils.hpp"
 
-geometry_msgs::msg::PoseStamped createPose(double x, double y)
+geometry_msgs::msg::PoseStamped createPoseStamped(double x, double y)
 {
   geometry_msgs::msg::PoseStamped pose;
   pose.pose.position.x = x;
@@ -32,6 +32,16 @@ geometry_msgs::msg::PoseStamped createPose(double x, double y)
   return pose;
 }
 
+geometry_msgs::msg::Pose createPose(double x, double y)
+{
+  geometry_msgs::msg::Pose pose;
+  pose.position.x = x;
+  pose.position.y = y;
+  pose.position.z = 0.0;
+  pose.orientation.w = 1.0;
+  return pose;
+}
+
 void generateCirclePath(
   nav_msgs::msg::Path & path,
   double center_x, double center_y, double radius,
@@ -40,12 +50,25 @@ void generateCirclePath(
   const double angle_step = (end_angle - start_angle) / (num_points - 1);
   for (int i = 0; i < num_points; ++i) {
     const double angle = start_angle + i * angle_step;
-    path.poses.push_back(createPose(
+    path.poses.push_back(createPoseStamped(
       center_x + radius * std::cos(angle),
       center_y + radius * std::sin(angle)));
   }
 }
 
+void generateCircleTrajectory(
+  std::vector<geometry_msgs::msg::Pose> & path,
+  double center_x, double center_y, double radius,
+  int num_points, double start_angle = 0.0, double end_angle = 2.0 * M_PI)
+{
+  const double angle_step = (end_angle - start_angle) / (num_points - 1);
+  for (int i = 0; i < num_points; ++i) {
+    const double angle = start_angle + i * angle_step;
+    path.push_back(createPose(
+      center_x + radius * std::cos(angle),
+      center_y + radius * std::sin(angle)));
+  }
+}
 class CloverleafPathTest : public ::testing::Test
 {
 protected:
@@ -57,14 +80,14 @@ class CloverleafPathTest : public ::testing::Test
     generateCirclePath(target_path, 0.0, 5.0, 5.0, 50);
 
     // Robot trajectory now also travels all three leaves, but with a radius of 4.8
-    nav_msgs::msg::Path robot_path;
-    generateCirclePath(robot_path, 5.0, 0.0, 4.8, 50);
-    generateCirclePath(robot_path, -5.0, 0.0, 4.8, 50);
-    generateCirclePath(robot_path, 0.0, 5.0, 4.8, 50);
-    robot_trajectory = robot_path.poses;
+    std::vector<geometry_msgs::msg::Pose> robot_path;
+    generateCircleTrajectory(robot_path, 5.0, 0.0, 4.8, 50);
+    generateCircleTrajectory(robot_path, -5.0, 0.0, 4.8, 50);
+    generateCircleTrajectory(robot_path, 0.0, 5.0, 4.8, 50);
+    robot_trajectory = robot_path;
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
 };
 
 class RetracingCircleTest : public ::testing::Test
@@ -74,25 +97,25 @@ class RetracingCircleTest : public ::testing::Test
   {
     generateCirclePath(target_path, 0.0, 0.0, 5.0, 50, 0.0, 2.0 * M_PI);
     generateCirclePath(target_path, 0.0, 0.0, 5.0, 50, 2.0 * M_PI, 0.0);
-    nav_msgs::msg::Path robot_path;
-    generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 0.0, 2.0 * M_PI);
-    generateCirclePath(robot_path, 0.0, 0.0, 5.2, 50, 2.0 * M_PI, 0.0);
-    robot_trajectory = robot_path.poses;
+    std::vector<geometry_msgs::msg::Pose> robot_path;
+    generateCircleTrajectory(robot_path, 0.0, 0.0, 5.2, 50, 0.0, 2.0 * M_PI);
+    generateCircleTrajectory(robot_path, 0.0, 0.0, 5.2, 50, 2.0 * M_PI, 0.0);
+    robot_trajectory = robot_path;
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
 };
 
 class ZigZagPathTest : public ::testing::Test
 {
 protected:
   void SetUp() override
   {
-    target_path.poses.push_back(createPose(0.0, 0.0));
-    target_path.poses.push_back(createPose(10.0, 0.0));
-    target_path.poses.push_back(createPose(10.0, 5.0));
-    target_path.poses.push_back(createPose(0.0, 5.0));
-    target_path.poses.push_back(createPose(0.0, 10.0));
+    target_path.poses.push_back(createPoseStamped(0.0, 0.0));
+    target_path.poses.push_back(createPoseStamped(10.0, 0.0));
+    target_path.poses.push_back(createPoseStamped(10.0, 5.0));
+    target_path.poses.push_back(createPoseStamped(0.0, 5.0));
+    target_path.poses.push_back(createPoseStamped(0.0, 10.0));
 
     robot_trajectory = {
       createPose(1.0, 0.2), createPose(5.0, 0.2), createPose(9.0, 0.2),
@@ -102,18 +125,18 @@ class ZigZagPathTest : public ::testing::Test
     };
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
 };
 
 class HairpinTurnTest : public ::testing::Test
 {
 protected:
   void SetUp() override
   {
-    target_path.poses.push_back(createPose(0.0, 1.0));
-    target_path.poses.push_back(createPose(10.0, 1.0));
-    target_path.poses.push_back(createPose(10.0, -1.0));
-    target_path.poses.push_back(createPose(0.0, -1.0));
+    target_path.poses.push_back(createPoseStamped(0.0, 1.0));
+    target_path.poses.push_back(createPoseStamped(10.0, 1.0));
+    target_path.poses.push_back(createPoseStamped(10.0, -1.0));
+    target_path.poses.push_back(createPoseStamped(0.0, -1.0));
 
     robot_trajectory = {
       createPose(1.0, 1.2), createPose(3.0, 1.2), createPose(5.0, 1.2),
@@ -124,17 +147,17 @@ class HairpinTurnTest : public ::testing::Test
     };
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
 };
 
 class CuttingCornerTest : public ::testing::Test
 {
 protected:
   void SetUp() override
   {
-    target_path.poses.push_back(createPose(0.0, 0.0));
-    target_path.poses.push_back(createPose(10.0, 0.0));
-    target_path.poses.push_back(createPose(10.0, 10.0));
+    target_path.poses.push_back(createPoseStamped(0.0, 0.0));
+    target_path.poses.push_back(createPoseStamped(10.0, 0.0));
+    target_path.poses.push_back(createPoseStamped(10.0, 10.0));
 
     robot_trajectory = {
       createPose(0.0, 0.2), createPose(2.0, 0.2), createPose(4.0, 0.2),
@@ -145,7 +168,7 @@ class CuttingCornerTest : public ::testing::Test
     expected_distances = {0.2, 0.2, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2, 0.2};
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
   std::vector<double> expected_distances;
 };
 
@@ -154,8 +177,8 @@ class RetracingPathTest : public ::testing::Test
 protected:
   void SetUp() override
   {
-    for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPose(i, 0.0));}
-    for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPose(i, 0.0));}
+    for (int i = 0; i <= 10; ++i) {target_path.poses.push_back(createPoseStamped(i, 0.0));}
+    for (int i = 9; i >= 0; --i) {target_path.poses.push_back(createPoseStamped(i, 0.0));}
     for (int i = 0; i <= 10; ++i) {
       robot_trajectory.push_back(createPose(i, 0.5));
     }
@@ -164,7 +187,7 @@ class RetracingPathTest : public ::testing::Test
     }
   }
   nav_msgs::msg::Path target_path;
-  std::vector<geometry_msgs::msg::PoseStamped> robot_trajectory;
+  std::vector<geometry_msgs::msg::Pose> robot_trajectory;
 };
 
 TEST(PathUtilsTest, EmptyAndSinglePointPaths)
@@ -176,7 +199,7 @@ TEST(PathUtilsTest, EmptyAndSinglePointPaths)
   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));
+  single_point_path.poses.push_back(createPoseStamped(0.0, 0.0));
   result = nav2_util::distance_from_path(single_point_path, robot_pose);
   EXPECT_NEAR(result.distance, 7.071, 0.01);
 }
@@ -303,8 +326,8 @@ TEST(PathUtilsWindowedTest, EdgeCases)
 {
   auto robot_pose = createPose(5.0, 5.0);
   nav_msgs::msg::Path test_path;
-  test_path.poses.push_back(createPose(0.0, 0.0));
-  test_path.poses.push_back(createPose(10.0, 0.0));
+  test_path.poses.push_back(createPoseStamped(0.0, 0.0));
+  test_path.poses.push_back(createPoseStamped(10.0, 0.0));
 
   auto result = nav2_util::distance_from_path(test_path, robot_pose, 0, 5.0);
   EXPECT_NEAR(result.distance, 5.0, 0.01);