diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 012c42a379c00..1c6295f3e70ef 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -729,10 +729,13 @@ std::vector cutOverlappedLanes( } // Step2. pick up only path points within drivable lanes + std::set 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;