fix(behavior_path_planner): pull over remove overlapping points (auto…

…warefoundation#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> Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
YoshiRi · Jan 11, 2023 · 032b9e7 · 032b9e7
1 parent 0463094
commit 032b9e7
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(