fix(behavior_path_planner_common): prevent duplicated point insertion…

… in cutOverlappedLanes (autowarefoundation#9363)

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

@@ -729,10 +729,13 @@ std::vector<DrivableLanes> cutOverlappedLanes(
   }
 
   // Step2. pick up only path points within drivable lanes
+  std::set<size_t> path_point_indices;
   for (const auto & drivable_lanes : shorten_lanes) {
     for (size_t i = start_point_idx; i < original_points.size(); ++i) {
-      if (is_point_in_drivable_lanes(drivable_lanes, original_points.at(i))) {
-        path.points.push_back(original_points.at(i));
+      const auto & p = original_points.at(i);
+      if (is_point_in_drivable_lanes(drivable_lanes, p) && path_point_indices.count(i) == 0) {
+        path.points.push_back(p);
+        path_point_indices.insert(i);
         continue;
       }
       start_point_idx = i;