From 5ca037fb49d4a89b081e35cd4a34cb21ae075a71 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 8 Mar 2024 18:36:14 +0900 Subject: [PATCH 1/3] fix union sometimes returning empty polygon Signed-off-by: Daniel Sanchez --- .../src/lane_departure_checker_node/lane_departure_checker.cpp | 2 +- .../behavior_path_start_planner_module/src/shift_pull_out.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 811e1652fcb4a..24b38b690a91a 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 @@ -339,7 +339,7 @@ std::optional LaneDepartureChecker::getFusedLaneletPoly std::vector lanelet_union_temp; boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - + if (lanelet_union_temp.empty()) continue; // Update merged_polygon by accumulating all merged results merged_polygon.clear(); for (const auto & temp_poly : lanelet_union_temp) { diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index f5674cfb288d0..274fe19e04618 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -110,7 +110,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); - + if (cropped_path.points.empty()) continue; shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); From 716d6bac53a9d4da7f51d4feea364c0c704c640a Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 11 Mar 2024 14:40:08 +0900 Subject: [PATCH 2/3] fix union of polygons Signed-off-by: Daniel Sanchez --- .../lane_departure_checker.hpp | 2 +- .../lane_departure_checker.cpp | 44 +++++++++++-------- .../behavior_planning.launch.xml | 10 +++-- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index be2411cd3268b..c658cf4497973 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -121,7 +121,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( 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 24b38b690a91a..c48383a17ab4b 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 @@ -321,33 +321,39 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - if (lanelets_distance_pair.size() == 1) - return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; - lanelet::BasicPolygon2d merged_polygon = - lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); - for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; - const auto & poly = route_lanelet.polygon2d().basicPolygon(); - - std::vector lanelet_union_temp; - boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - if (lanelet_union_temp.empty()) continue; - // 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()); - } + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); } - if (merged_polygon.empty()) return std::nullopt; - return merged_polygon; + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); }(); return fused_lanelets; diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d2aa009d9c699..728b5666a3cef 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -181,8 +181,7 @@ /> - - + @@ -217,8 +216,11 @@ - - + + + + + From afc0f07ae060214d0ddf06c4773dc2ef520f2ec2 Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Mon, 11 Mar 2024 14:48:37 +0900 Subject: [PATCH 3/3] fix back launcher Signed-off-by: Daniel Sanchez --- .../behavior_planning/behavior_planning.launch.xml | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 728b5666a3cef..d2aa009d9c699 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -181,7 +181,8 @@ /> - + + @@ -216,11 +217,8 @@ - - - - - + +