feat(motion_utils): generalize overlap functions

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -26,37 +26,6 @@
 #include <stdexcept>
 #include <vector>
 
-namespace
-{
-template <class T>
-std::vector<geometry_msgs::msg::Point> removeOverlapPoints(const T & points, const size_t & idx)
-{
-  std::vector<geometry_msgs::msg::Point> dst;
-
-  for (const auto & pt : points) {
-    dst.push_back(tier4_autoware_utils::getPoint(pt));
-  }
-
-  if (points.empty()) {
-    return dst;
-  }
-
-  constexpr double eps = 1.0E-08;
-  size_t i = idx;
-  while (i != dst.size() - 1) {
-    const auto p = tier4_autoware_utils::getPoint(dst.at(i));
-    const auto p_next = tier4_autoware_utils::getPoint(dst.at(i + 1));
-    const Eigen::Vector3d v{p_next.x - p.x, p_next.y - p.y, 0.0};
-    if (v.norm() < eps) {
-      dst.erase(dst.begin() + i + 1);
-    } else {
-      ++i;
-    }
-  }
-  return dst;
-}
-}  // namespace
-
 namespace motion_utils
 {
 template <class T>
@@ -122,6 +91,33 @@ boost::optional<bool> isDrivingForwardWithTwist(const T points_with_twist)
   return isDrivingForward(points_with_twist);
 }
 
+template <class T>
+T removeOverlapPoints(const T & points, const size_t & start_idx = 0)
+{
+  if (points.size() < start_idx + 1) {
+    return points;
+  }
+
+  T dst;
+
+  for (size_t i = 0; i <= start_idx; ++i) {
+    dst.push_back(points.at(i));
+  }
+
+  constexpr double eps = 1.0E-08;
+  for (size_t i = start_idx + 1; i < points.size(); ++i) {
+    const auto prev_p = tier4_autoware_utils::getPoint(dst.back());
+    const auto curr_p = tier4_autoware_utils::getPoint(points.at(i));
+    const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p);
+    if (dist < eps) {
+      continue;
+    }
+    dst.push_back(points.at(i));
+  }
+
+  return dst;
+}
+
 template <class T>
 boost::optional<size_t> searchZeroVelocityIndex(
   const T & points_with_twist, const size_t src_idx, const size_t dst_idx)

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -4397,3 +4397,130 @@ TEST(trajectory, calcSignedArcLengthFromPointAndSegmentIndexToPointIndex)
     EXPECT_NEAR(calcSignedArcLength(traj.points, 4, p2, 1), -1.7, epsilon);
   }
 }
+
+TEST(trajectory, removeOverlapPoints)
+{
+  using motion_utils::removeOverlapPoints;
+
+  const auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
+  const auto removed_traj = removeOverlapPoints(traj.points, 0);
+  EXPECT_EQ(traj.points.size(), removed_traj.size());
+  for (size_t i = 0; i < traj.points.size(); ++i) {
+    EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
+    EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
+    EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
+    EXPECT_NEAR(
+      traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
+    EXPECT_NEAR(
+      traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
+    EXPECT_NEAR(
+      traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
+    EXPECT_NEAR(
+      traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
+    EXPECT_NEAR(
+      traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
+      epsilon);
+  }
+
+  // No overlap points
+  {
+    const auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
+    for (size_t start_idx = 0; start_idx < 10; ++start_idx) {
+      const auto removed_traj = removeOverlapPoints(traj.points, start_idx);
+      EXPECT_EQ(traj.points.size(), removed_traj.size());
+      for (size_t i = 0; i < traj.points.size(); ++i) {
+        EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
+        EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
+        EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
+        EXPECT_NEAR(
+          traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
+        EXPECT_NEAR(
+          traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
+        EXPECT_NEAR(
+          traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
+        EXPECT_NEAR(
+          traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
+          epsilon);
+      }
+    }
+  }
+
+  // Overlap points from certain point
+  {
+    auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
+    traj.points.at(5) = traj.points.at(6);
+    const auto removed_traj = removeOverlapPoints(traj.points);
+
+    EXPECT_EQ(traj.points.size() - 1, removed_traj.size());
+    for (size_t i = 0; i < 6; ++i) {
+      EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
+      EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
+      EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
+        epsilon);
+    }
+
+    for (size_t i = 6; i < 9; ++i) {
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i + 1).longitudinal_velocity_mps,
+        removed_traj.at(i).longitudinal_velocity_mps, epsilon);
+    }
+  }
+
+  // Overlap points from certain point
+  {
+    auto traj = generateTestTrajectory<Trajectory>(10, 1.0, 1.0);
+    traj.points.at(5) = traj.points.at(6);
+    const auto removed_traj = removeOverlapPoints(traj.points, 6);
+
+    EXPECT_EQ(traj.points.size(), removed_traj.size());
+    for (size_t i = 0; i < traj.points.size(); ++i) {
+      EXPECT_NEAR(traj.points.at(i).pose.position.x, removed_traj.at(i).pose.position.x, epsilon);
+      EXPECT_NEAR(traj.points.at(i).pose.position.y, removed_traj.at(i).pose.position.y, epsilon);
+      EXPECT_NEAR(traj.points.at(i).pose.position.z, removed_traj.at(i).pose.position.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.x, removed_traj.at(i).pose.orientation.x, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.y, removed_traj.at(i).pose.orientation.y, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.z, removed_traj.at(i).pose.orientation.z, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).pose.orientation.w, removed_traj.at(i).pose.orientation.w, epsilon);
+      EXPECT_NEAR(
+        traj.points.at(i).longitudinal_velocity_mps, removed_traj.at(i).longitudinal_velocity_mps,
+        epsilon);
+    }
+  }
+
+  // Empty Points
+  {
+    const Trajectory traj;
+    const auto removed_traj = removeOverlapPoints(traj.points);
+    EXPECT_TRUE(removed_traj.empty());
+  }
+}

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -224,7 +224,7 @@ geometry_msgs::msg::PointStamped calcNearestCollisionPoint(
   const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward)
 {
-  std::array<geometry_msgs::msg::Point, 2> segment_points;
+  std::vector<geometry_msgs::msg::Point> segment_points(2);
   if (first_within_idx == 0) {
     const auto & traj_front_pose = decimated_traj.points.at(0).pose;
     const auto front_pos = tier4_autoware_utils::calcOffsetPose(