From 5305676ea7745e47949468fdc80ddae42737146f Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 9 Feb 2024 16:18:19 +0900 Subject: [PATCH] merge all union polygon Signed-off-by: kyoichi-sugahara --- .../lane_departure_checker.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index ccff023afdbde..dd8cf4d258cb8 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -331,7 +331,6 @@ std::optional LaneDepartureChecker::getFusedLaneletPoly if (lanelets_distance_pair.size() == 1) return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); - std::vector lanelet_union; lanelet::BasicPolygon2d merged_polygon = lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { @@ -340,11 +339,16 @@ std::optional LaneDepartureChecker::getFusedLaneletPoly std::vector lanelet_union_temp; boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - merged_polygon = lanelet_union_temp.back(); - lanelet_union = lanelet_union_temp; + + // Update merged_polygon by accumulating all merged results + merged_polygon.clear(); + for (const auto & temp_poly : lanelet_union_temp) { + merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); + } + } - if (lanelet_union.empty()) return std::nullopt; - return lanelet_union.back(); + if(merged_polygon.empty()) return std::nullopt; + return merged_polygon; }(); return fused_lanelets;