feat(intersection): ignore occlusion behind blocking vehicle (autowar…

…efoundation#5122)

Signed-off-by: Mamoru Sobue <[email protected]>

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                                                                                    |

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(

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<autoware_auto_perception_msgs::msg::PredictedObject> parked_attention_objects;
+  std::vector<autoware_auto_perception_msgs::msg::PredictedObject> 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<util::DiscretizedLane> & lane_divisions,
-  [[maybe_unused]] const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
     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<std::vector<cv::Point>> 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<unsigned char>(height - 1 - idx_y, idx_x) = 0;
+          continue;
+        }
+        if (attention_mask.at<unsigned char>(height - 1 - idx_y, idx_x) == BLOCKED) {
+          blocking_vehicle_found = true;
+          occlusion_mask.at<unsigned char>(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<unsigned char>(height - 1 - idx_y, idx_x) == 255) {
+        const auto pixel = occlusion_mask.at<unsigned char>(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 = {

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;

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,

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -50,6 +50,7 @@ struct DebugData
   std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
   std::optional<std::pair<geometry_msgs::msg::Point, geometry_msgs::msg::Point>>
     nearest_occlusion_projection{std::nullopt};
+  autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects;
 };
 
 struct InterpolatedPathInfo