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 index 377a7bf280642..e3a6f8ee6c809 100644 --- 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 & detection_areas, + lanelet::ConstLanelets adjacent_lanelets, const lanelet::CompoundPolygon3d & first_detection_area, const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const std::pair & 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 index 94a6750b83006..7571ba86728f7 100644 --- 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 @@ -946,7 +947,7 @@ bool IntersectionModule::checkFrontVehicleDeceleration( bool IntersectionModule::isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & 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 & lane_interval_ip, const std::vector & lane_divisions, @@ -1014,6 +1015,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> 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 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 adjacent_lane_cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / reso); + const int idx_y = static_cast((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]