feat(motion_utils): generalize overlap functions (autowarefoundation#…

…1871) Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com>
tier4 · Oct 17, 2022 · 2b19a2d · 2b19a2d
1 parent e6b7bd9
commit 2b19a2d
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 32 deletions.
diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/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>
@@ -119,6 +88,33 @@ 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)

diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/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(