diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 5f26ef34c1186..e8a823a7e790e 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -94,6 +94,16 @@ If a stopline is associated with the intersection lane, that line is used as the To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +### Occlusion detection + +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. + +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. + +![occlusion_detection](./docs/occlusion_grid.drawio.svg) + +If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. + ### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg new file mode 100644 index 0000000000000..fc09a4212070a --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -0,0 +1,2616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + default stop line + +
+
+
+ + default stop line + + + + + + + + +
+
+
+ + occlusion peeking line + +
+
+
+ + occlusion peeking line + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + +
+
+
+ occlusion attention area +
+
+
+ + occlusion attent... + + + + + + + + + + + +
+
+
+ + + stopped vehicle on attention area +
+ (blocking vehicle) + + +
+
+
+ + stopped vehicle on attentio... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + The cells behind blocking vehicles +
+ are not marked as occluded +
+ + +
+
+
+ + The cells behind blocking vehicles... + + + + + + + + +
+
+
+ + + mark the cells which is unknown and +
+ belong to attention area are +
+ marked as occluded + +
+ +
+
+
+ + mark the cells which is unknown and... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 13 +
+
+
+ + 13 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 4 +
+
+ +
+
+
+ + 4... + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 55987600a22dc..7731d6490fb94 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -221,6 +221,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, + 0.99, 0.6), + &debug_marker_array, now); + /* appendMarkerArray( createPoseMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ca1cbfdd0ccc7..89bb65f403095 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -929,28 +929,29 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector parked_attention_objects; + std::vector blocking_attention_objects; std::copy_if( target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(parked_attention_objects), + std::back_inserter(blocking_attention_objects), [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { return std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); + debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - parked_attention_objects, occlusion_dist_thr) + blocking_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, @@ -1064,9 +1065,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin); + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); if (is_in_adjacent_lanelets) { continue; } @@ -1078,17 +1081,19 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT if (is_in_intersection_area) { target_objects.objects.push_back(object); } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { target_objects.objects.push_back(object); } } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } @@ -1240,7 +1245,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - [[maybe_unused]] const std::vector & + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { @@ -1265,6 +1270,13 @@ bool IntersectionModule::isOcclusionCleared( const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); @@ -1351,8 +1363,39 @@ bool IntersectionModule::isOcclusionCleared( cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + } // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; @@ -1397,19 +1440,11 @@ bool IntersectionModule::isOcclusionCleared( } // (4.1) re-draw occluded cells using valid_contours occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (unsigned i = 0; i < valid_contours.size(); ++i) { + for (const auto & valid_contour : valid_contours) { // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); } - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - // (5) find distance // (5.1) discretize path_ip with resolution for computational cost LineString2d path_linestring; @@ -1496,7 +1531,11 @@ bool IntersectionModule::isOcclusionCleared( const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); // TODO(Mamoru Sobue): add handling for blocking vehicles if (!valid) continue; - if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 855fd8472d76d..28be8d0a6a351 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1056,17 +1056,18 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( } bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin) + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold) { for (const auto & ll : target_lanelets) { - if (!lanelet::utils::isInLanelet(pose, ll, margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return true; @@ -1075,6 +1076,14 @@ bool checkAngleForTargetLanelets( if (std::fabs(angle_diff) < detection_area_angle_thr) { return true; } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && + (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return true; + } } } return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f02362e3f803d..7ba894bd9d32a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -136,9 +136,10 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin = 0.0); + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold); void cutPredictPathWithDuration( autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4135884f03c74..7e5bff89485ed 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; }; struct InterpolatedPathInfo