feat(motion_utils): add a resample funciton (#2372)

* feat(motion_utils): add new resample function

Signed-off-by: yutaka <purewater0901@gmail.com>

* fix format

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

Signed-off-by: yutaka <purewater0901@gmail.com>

common/motion_utils/include/motion_utils/resample/resample.hpp

@@ -56,6 +56,23 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
   const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
   const bool use_lerp_for_z = true);
 
+/**
+ * @brief A resampling function for a path(poses). Note that in a default setting, position xy are
+ *        resampled by spline interpolation, position z are resampled by linear interpolation, and
+ *        orientation of the resampled path are calculated by a forward difference method
+ *        based on the interpolated position x and y.
+ * @param input_path input path(poses) to resample
+ * @param resample_interval resampling interval
+ * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
+ *        y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ *        Otherwise, it uses spline interpolation
+ * @return resampled path(poses)
+ */
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points, const double resampled_interval,
+  const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true);
+
 /**
  * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
  *        are resampled by spline interpolation, position z are resampled by linear interpolation,

common/motion_utils/src/resample/resample.cpp

@@ -90,6 +90,31 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
   return resampled_points;
 }
 
+std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
+  const std::vector<geometry_msgs::msg::Pose> & points, const double resample_interval,
+  const bool use_lerp_for_xy, const bool use_lerp_for_z)
+{
+  const double input_length = motion_utils::calcArcLength(points);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_length; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl;
+    return points;
+  }
+
+  // Insert terminal point
+  if (input_length - resampling_arclength.back() < motion_utils::overlap_threshold) {
+    resampling_arclength.back() = input_length;
+  } else {
+    resampling_arclength.push_back(input_length);
+  }
+
+  return resamplePoseVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z);
+}
+
 autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
   const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,

common/motion_utils/test/src/resample/test_resample.cpp

@@ -174,6 +174,49 @@ std::vector<double> generateArclength(const size_t num_points, const double inte
 }
 }  // namespace
 
+TEST(resample_vector_pose, resample_by_same_interval)
+{
+  using geometry_msgs::msg::Pose;
+  using motion_utils::resamplePoseVector;
+
+  std::vector<Pose> path(10);
+  for (size_t i = 0; i < 10; ++i) {
+    path.at(i) = createPose(i * 1.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+  }
+
+  // same interval
+  {
+    const auto resampled_path = resamplePoseVector(path, 1.0);
+    EXPECT_EQ(path.size(), resampled_path.size());
+    for (size_t i = 0; i < path.size(); ++i) {
+      const auto & p = resampled_path.at(i);
+      const auto & ans_p = path.at(i);
+      EXPECT_NEAR(p.position.x, ans_p.position.x, epsilon);
+      EXPECT_NEAR(p.position.y, ans_p.position.y, epsilon);
+      EXPECT_NEAR(p.position.z, ans_p.position.z, epsilon);
+      EXPECT_NEAR(p.orientation.x, ans_p.orientation.x, epsilon);
+      EXPECT_NEAR(p.orientation.y, ans_p.orientation.y, epsilon);
+      EXPECT_NEAR(p.orientation.z, ans_p.orientation.z, epsilon);
+      EXPECT_NEAR(p.orientation.w, ans_p.orientation.w, epsilon);
+    }
+  }
+
+  // random
+  {
+    const auto resampled_path = resamplePoseVector(path, 0.5);
+    for (size_t i = 0; i < path.size(); ++i) {
+      const auto & p = resampled_path.at(i);
+      EXPECT_NEAR(p.position.x, 0.5 * i, epsilon);
+      EXPECT_NEAR(p.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.orientation.w, 1.0, epsilon);
+    }
+  }
+}
+
 TEST(resample_path_with_lane_id, resample_path_by_vector)
 {
   using motion_utils::resamplePath;