fix(behavior_path_planner): pull over remove overlapping points (#2246)

* fix(behavior_path_planner): pull over remove overlapping points Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * check empty Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Nov 9, 2022 · f1efcc2 · f1efcc2
1 parent 2a48d14
commit f1efcc2
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp
@@ -482,9 +482,12 @@ PathWithLaneId GeometricParallelParking::generateArcPath(
 
   // insert the last point exactly
   const auto p = generateArcPathPoint(center, radius, end_yaw, is_left_turn, is_forward);
-  path.points.push_back(p);
+  constexpr double min_dist = 0.01;
+  if (path.points.empty() || calcDistance2d(path.points.back(), p) > min_dist) {
+    path.points.push_back(p);
+  }
 
-  return removeOverlappingPoints(path);
+  return path;
 }
 
 PathPointWithLaneId GeometricParallelParking::generateArcPathPoint(