feat(intersection): clear costmap on adjacent lanes (autowarefoundati…

…on#3553) clear costmap on adjacent lanes Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: Mingyu Li <[email protected]>
Mingyu1991 · Jun 26, 2023 · f3e7265 · f3e7265
1 parent ff04fbd
commit f3e7265
Show file tree

Hide file tree

Showing 2 changed files with 38 additions and 3 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
@@ -323,6 +323,7 @@ class IntersectionModule : public SceneModuleInterface
   bool isOcclusionCleared(
     const nav_msgs::msg::OccupancyGrid & occ_grid,
     const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
+    lanelet::ConstLanelets adjacent_lanelets,
     const lanelet::CompoundPolygon3d & first_detection_area,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip,
     const std::pair<size_t, size_t> & lane_interval,

diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -250,8 +250,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason *
   const bool is_occlusion_cleared =
     (enable_occlusion_detection_ && first_detection_area && !occlusion_attention_lanelets.empty())
       ? isOcclusionCleared(
-          *planner_data_->occupancy_grid, occlusion_attention_area, first_detection_area.value(),
-          path_ip, lane_interval_ip, detection_divisions_.value(), occlusion_dist_thr)
+          *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
+          first_detection_area.value(), path_ip, lane_interval_ip, detection_divisions_.value(),
+          occlusion_dist_thr)
       : true;
   const auto occlusion_peeking_line_idx_opt =
     first_detection_area
@@ -948,7 +949,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration(
 bool IntersectionModule::isOcclusionCleared(
   const nav_msgs::msg::OccupancyGrid & occ_grid,
   const std::vector<lanelet::CompoundPolygon3d> & detection_areas,
-  const lanelet::CompoundPolygon3d & first_detection_area,
+  lanelet::ConstLanelets adjacent_lanelets, const lanelet::CompoundPolygon3d & first_detection_area,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip,
   const std::pair<size_t, size_t> & lane_interval_ip,
   const std::vector<util::DetectionLaneDivision> & lane_divisions,
@@ -1016,6 +1017,39 @@ bool IntersectionModule::isOcclusionCleared(
   for (const auto & poly : detection_area_cv_polygons) {
     cv::fillPoly(detection_mask, poly, cv::Scalar(255), cv::LINE_AA);
   }
+  // (1.1)
+  // reset adjacent_lanelets area to 0 on detection_mask
+  std::vector<std::vector<cv::Point>> adjacent_lane_cv_polygons;
+  for (const auto & adjacent_lanelet : adjacent_lanelets) {
+    const auto area2d = adjacent_lanelet.polygon2d().basicPolygon();
+    Polygon2d area2d_poly;
+    for (const auto & p : area2d) {
+      area2d_poly.outer().emplace_back(p.x(), p.y());
+    }
+    area2d_poly.outer().push_back(area2d_poly.outer().front());
+    bg::correct(area2d_poly);
+    std::vector<Polygon2d> common_areas;
+    bg::intersection(area2d_poly, grid_poly, common_areas);
+    if (common_areas.empty()) {
+      continue;
+    }
+    for (size_t i = 0; i < common_areas.size(); ++i) {
+      common_areas[i].outer().push_back(common_areas[i].outer().front());
+      bg::correct(common_areas[i]);
+    }
+    for (const auto & common_area : common_areas) {
+      std::vector<cv::Point> adjacent_lane_cv_polygon;
+      for (const auto & p : common_area.outer()) {
+        const int idx_x = static_cast<int>((p.x() - origin.x) / reso);
+        const int idx_y = static_cast<int>((p.y() - origin.y) / reso);
+        adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y);
+      }
+      adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon);
+    }
+  }
+  for (const auto & poly : adjacent_lane_cv_polygons) {
+    cv::fillPoly(detection_mask, poly, cv::Scalar(0), cv::LINE_AA);
+  }
 
   // (2) prepare unknown mask
   // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accesed by img[y, x]