fix(resample): remove duplicated poinnt before resampling

Signed-off-by: Takamasa Horibe <[email protected]>
tier4 · Sep 14, 2023 · 096f1ca · 096f1ca
1 parent 87e9ebf
commit 096f1ca
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp
@@ -109,13 +109,16 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
 }
 
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
-  const std::vector<geometry_msgs::msg::Pose> & points,
+  const std::vector<geometry_msgs::msg::Pose> & points_raw,
   const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
+  // Remove overlap points for resampling
+  const auto points = motion_utils::removeOverlapPoints(points_raw);
+
   // validate arguments
   if (!resample_utils::validate_arguments(points, resampled_arclength)) {
-    return points;
+    return points_raw;
   }
 
   std::vector<geometry_msgs::msg::Point> position(points.size());