fix(lane_departure_checker): empty lanelet polygon (#6588)

* fix union sometimes returning empty polygon Signed-off-by: Daniel Sanchez <[email protected]> * fix union of polygons Signed-off-by: Daniel Sanchez <[email protected]> * fix back launcher Signed-off-by: Daniel Sanchez <[email protected]> --------- Signed-off-by: Daniel Sanchez <[email protected]> Signed-off-by: Kotaro Yoshimoto <[email protected]>
autowarefoundation · Mar 12, 2024 · e6482b7 · e6482b7
1 parent 7389a86
commit e6482b7
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 21 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
@@ -121,7 +121,7 @@ class LaneDepartureChecker
   std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
-  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+  std::optional<tier4_autoware_utils::Polygon2d> 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
@@ -321,33 +321,39 @@ std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLanele
     lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
 }
 
-std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+std::optional<tier4_autoware_utils::Polygon2d> 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<lanelet::BasicPolygon2d> {
+  auto fused_lanelets = [&]() -> std::optional<tier4_autoware_utils::Polygon2d> {
     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::BasicPolygon2d> lanelet_union_temp;
-      boost::geometry::union_(poly, merged_polygon, 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());
-      }
+      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/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<PullOutPath> 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();