feat(intersection_occlusion): ignore occlusion behind parked vehicles on the attention lane

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -47,6 +47,7 @@
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
         possible_object_bbox: [1.0, 2.0] # [m x m]
+        ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h
 
       enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection: true

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -115,6 +115,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
   ip.occlusion.possible_object_bbox =
     node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
+  ip.occlusion.ignore_parked_vehicle_speed_threshold =
+    node.declare_parameter<double>(ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -799,9 +799,10 @@
                               ? 0.0
                               : (planner_param_.collision_detection.state_transit_margin_time -
                                  collision_state_machine_.getDuration());
+  const auto target_objects =
+    filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area);
   const bool has_collision = checkCollision(
-    *path, attention_lanelets, adjacent_lanelets, intersection_area, ego_lane_with_next_lane,
-    closest_idx, time_delay, tl_arrow_solid_on);
+    *path, target_objects, ego_lane_with_next_lane, closest_idx, time_delay, tl_arrow_solid_on);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
@@ -822,12 +823,19 @@
   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::copy_if(
+    target_objects.objects.begin(), target_objects.objects.end(),
+    std::back_inserter(parked_attention_objects),
+    [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) {
+      return std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) <= thresh;
+    });
   const bool is_occlusion_cleared =
     (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on)
       ? isOcclusionCleared(
           *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets,
           first_attention_area.value(), interpolated_path_info,
-          occlusion_attention_divisions_.value(), occlusion_dist_thr)
+          occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr)
       : true;
 
   // check safety
@@ -909,13 +917,10 @@
   return is_stuck;
 }
 
-bool IntersectionModule::checkCollision(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects(
   const lanelet::ConstLanelets & attention_area_lanelets,
   const lanelet::ConstLanelets & adjacent_lanelets,
-  const std::optional<Polygon2d> & intersection_area,
-  const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx,
-  const double time_delay, const bool tl_arrow_solid_on)
+  const std::optional<Polygon2d> & intersection_area) const
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
@@ -962,6 +967,17 @@
       target_objects.objects.push_back(object);
     }
   }
+  return target_objects;
+}
+
+bool IntersectionModule::checkCollision(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
+  const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx,
+  const double time_delay, const bool tl_arrow_solid_on)
+{
+  using lanelet::utils::getArcCoordinates;
+  using lanelet::utils::getPolygonFromArcLength;
 
   // check collision between target_objects predicted path and ego lane
   // cut the predicted path at passing_time
@@ -970,6 +986,7 @@
     planner_param_.common.intersection_velocity,
     planner_param_.collision_detection.minimum_ego_predicted_velocity);
   const double passing_time = time_distance_array.back().first;
+  auto target_objects = objects;
   util::cutPredictPathWithDuration(&target_objects, clock_, passing_time);
 
   const auto closest_arc_coords = getArcCoordinates(
@@ -1097,150 +1114,153 @@
   const lanelet::ConstLanelets & adjacent_lanelets,
   const lanelet::CompoundPolygon3d & first_attention_area,
   const util::InterpolatedPathInfo & interpolated_path_info,
-  const std::vector<util::DescritizedLane> & lane_divisions, const double occlusion_dist_thr)
+  const std::vector<util::DescritizedLane> & lane_divisions,
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & parked_attention_objects,
+  const double occlusion_dist_thr)
 {
   const auto & path_ip = interpolated_path_info.path;
   const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value();
 
   const auto first_attention_area_idx =
     util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area);
   if (!first_attention_area_idx) {
     return false;
   }
 
   const auto first_inside_attention_idx_ip_opt =
     util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area);
   const std::pair<size_t, size_t> lane_attention_interval_ip =
     first_inside_attention_idx_ip_opt
       ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip))
       : lane_interval_ip;
 
   const int width = occ_grid.info.width;
   const int height = occ_grid.info.height;
   const double resolution = occ_grid.info.resolution;
   const auto & origin = occ_grid.info.origin.position;
 
   // NOTE: interesting area is set to 0 for later masking
   cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0));
   cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0));
 
   // (1) prepare detection area mask
+  // attention: 255
+  // non-attention: 0
   Polygon2d grid_poly;
   grid_poly.outer().emplace_back(origin.x, origin.y);
   grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y);
   grid_poly.outer().emplace_back(
     origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution);
   grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution);
   grid_poly.outer().emplace_back(origin.x, origin.y);
   bg::correct(grid_poly);
 
   std::vector<std::vector<cv::Point>> attention_area_cv_polygons;
   for (const auto & attention_area : attention_areas) {
     const auto area2d = lanelet::utils::to2D(attention_area);
     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> attention_area_cv_polygon;
       for (const auto & p : common_area.outer()) {
         const int idx_x = static_cast<int>((p.x() - origin.x) / resolution);
         const int idx_y = static_cast<int>((p.y() - origin.y) / resolution);
         attention_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y);
       }
       attention_area_cv_polygons.push_back(attention_area_cv_polygon);
     }
   }
   for (const auto & poly : attention_area_cv_polygons) {
-    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_4);
+    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA);
   }
   // (1.1)
   // reset adjacent_lanelets area to 0 on attention_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 = std::floor<int>((p.x() - origin.x) / resolution);
         const int idx_y = std::floor<int>((p.y() - origin.y) / resolution);
         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(attention_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 accessed by img[y, x]
   for (int x = 0; x < width; x++) {
     for (int y = 0; y < height; y++) {
       const int idx = y * width + x;
       const unsigned char intensity = occ_grid.data.at(idx);
       if (
         planner_param_.occlusion.free_space_max <= intensity &&
         intensity < planner_param_.occlusion.occupied_min) {
         unknown_mask.at<unsigned char>(height - 1 - y, x) = 255;
       }
     }
   }
 
   // (3) occlusion mask
   cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0));
   cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw);
   // (3.1) apply morphologyEx
   cv::Mat occlusion_mask;
   const int morph_size = static_cast<int>(planner_param_.occlusion.denoise_kernel / resolution);
   cv::morphologyEx(
     occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN,
     cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size)));
 
   // (4) create distance grid
   // value: 0 - 254: signed distance representing [distance_min, distance_max]
   // 255: undefined value
   const double distance_max = std::hypot(width * resolution / 2, height * resolution / 2);
   const double distance_min = -distance_max;
   const int undef_pixel = 255;
   const int max_cost_pixel = 254;
-
   auto dist2pixel = [=](const double dist) {
     return std::min(
       max_cost_pixel,
       static_cast<int>((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel));
   };
   auto pixel2dist = [=](const int pixel) {
     return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min;
   };
-
   const int zero_dist_pixel = dist2pixel(0.0);
+  const int parked_vehicle_pixel = zero_dist_pixel - 1;  // magic
 
   auto coord2index = [&](const double x, const double y) {
     const int idx_x = (x - origin.x) / resolution;
@@ -1253,6 +1273,7 @@
   cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel));
   cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1));
 
+  // (4.1) fill zero_dist_pixel on the path
   const auto [lane_start, lane_end] = lane_attention_interval_ip;
   for (int i = static_cast<int>(lane_end); i >= static_cast<int>(lane_start); i--) {
     const auto & path_pos = path_ip.points.at(i).point.pose.position;
@@ -1264,6 +1285,31 @@
     projection_ind_grid.at<int>(height - 1 - idx_y, idx_x) = i;
   }
 
+  // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative)
+  for (const auto & parked_attention_object : parked_attention_objects) {
+    const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object);
+    std::vector<Polygon2d> common_areas;
+    bg::intersection(obj_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]);
+    }
+    std::vector<std::vector<cv::Point>> parked_attention_object_area_cv_polygons;
+    for (const auto & common_area : common_areas) {
+      std::vector<cv::Point> parked_attention_object_area_cv_polygon;
+      for (const auto & p : common_area.outer()) {
+        const int idx_x = static_cast<int>((p.x() - origin.x) / resolution);
+        const int idx_y = static_cast<int>((p.y() - origin.y) / resolution);
+        parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y);
+      }
+      parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon);
+    }
+    for (const auto & poly : parked_attention_object_area_cv_polygons) {
+      cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA);
+    }
+  }
+
   for (const auto & lane_division : lane_divisions) {
     const auto & divisions = lane_division.divisions;
     for (const auto & division : divisions) {
@@ -1272,8 +1318,9 @@
       int projection_ind = -1;
       std::optional<std::tuple<double, double, double, int>> cost_prev_checkpoint =
         std::nullopt;  // cost, x, y, projection_ind
-      for (const auto & point : division) {
-        const auto [valid, idx_x, idx_y] = coord2index(point.x(), point.y());
+      for (auto point = division.begin(); point != division.end(); point++) {
+        const double x = point->x(), y = point->y();
+        const auto [valid, idx_x, idx_y] = coord2index(x, y);
         // exited grid just now
         if (is_in_grid && !valid) break;
 
@@ -1296,16 +1343,27 @@
           zero_dist_cell_found = true;
           projection_ind = projection_ind_grid.at<int>(height - 1 - idx_y, idx_x);
           assert(projection_ind >= 0);
-          cost_prev_checkpoint = std::make_optional<std::tuple<double, double, double, int>>(
-            0.0, point.x(), point.y(), projection_ind);
+          cost_prev_checkpoint =
+            std::make_optional<std::tuple<double, double, double, int>>(0.0, x, y, projection_ind);
           continue;
         }
 
+        // hit positive parked vehicle
+        if (zero_dist_cell_found && pixel == parked_vehicle_pixel) {
+          while (point != division.end()) {
+            const double x = point->x(), y = point->y();
+            const auto [valid, idx_x, idx_y] = coord2index(x, y);
+            if (valid) occlusion_mask.at<unsigned char>(height - 1 - idx_y, idx_x) = 0;
+            point++;
+          }
+          break;
+        }
+
         if (zero_dist_cell_found) {
           // finally traversed to defined cell (first half)
           const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] =
             cost_prev_checkpoint.value();
-          const double dy = point.y() - prev_checkpoint_y, dx = point.x() - prev_checkpoint_x;
+          const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x;
           double new_dist = prev_cost + std::hypot(dy, dx);
           const int new_projection_ind = projection_ind_grid.at<int>(height - 1 - idx_y, idx_x);
           const double cur_dist = pixel2dist(pixel);
@@ -1318,7 +1376,7 @@
           projection_ind_grid.at<int>(height - 1 - idx_y, idx_x) = projection_ind;
           distance_grid.at<unsigned char>(height - 1 - idx_y, idx_x) = dist2pixel(new_dist);
           cost_prev_checkpoint = std::make_optional<std::tuple<double, double, double, int>>(
-            new_dist, point.x(), point.y(), projection_ind);
+            new_dist, x, y, projection_ind);
         }
       }
     }
@@ -1329,7 +1387,7 @@
   const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution;
   const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y;
   std::vector<std::vector<cv::Point>> contours;
-  cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+  cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
   std::vector<std::vector<cv::Point>> valid_contours;
   for (const auto & contour : contours) {
     std::vector<cv::Point> valid_contour;
@@ -1346,11 +1404,18 @@
     cv::approxPolyDP(
       valid_contour, approx_contour,
       std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true);
-    if (approx_contour.size() <= 1) continue;
+    if (approx_contour.size() <= 2) continue;
     // check area
     const double poly_area = cv::contourArea(approx_contour);
     if (poly_area < possible_object_area) continue;
-
+    // check bounding box size
+    const auto bbox = cv::minAreaRect(approx_contour);
+    if (const auto size = bbox.size; std::min(size.height, size.width) <
+                                       std::min(possible_object_bbox_x, possible_object_bbox_y) ||
+                                     std::max(size.height, size.width) <
+                                       std::max(possible_object_bbox_x, possible_object_bbox_y)) {
+      continue;
+    }
     valid_contours.push_back(approx_contour);
     geometry_msgs::msg::Polygon polygon_msg;
     geometry_msgs::msg::Point32 point_msg;

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -107,6 +107,7 @@
       double max_vehicle_velocity_for_rss;
       double denoise_kernel;
       std::vector<double> possible_object_bbox;
+      double ignore_parked_vehicle_speed_threshold;
     } occlusion;
   };
 
@@ -216,7 +217,7 @@
 
   // for occlusion detection
   const bool enable_occlusion_detection_;
   std::optional<std::vector<util::DescritizedLane>> occlusion_attention_divisions_;
   // OcclusionState prev_occlusion_state_ = OcclusionState::NONE;
   StateMachine collision_state_machine_;     //! for stable collision checking
   StateMachine before_creep_state_machine_;  //! for two phase stop
@@ -247,11 +248,14 @@
     const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
     const util::IntersectionStopLines & intersection_stop_lines);
 
-  bool checkCollision(
-    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+  autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects(
     const lanelet::ConstLanelets & attention_area_lanelets,
     const lanelet::ConstLanelets & adjacent_lanelets,
-    const std::optional<Polygon2d> & intersection_area,
+    const std::optional<Polygon2d> & intersection_area) const;
+
+  bool checkCollision(
+    const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
+    const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects,
     const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx,
     const double time_delay, const bool tl_arrow_solid_on);
 
@@ -261,7 +265,10 @@
     const lanelet::ConstLanelets & adjacent_lanelets,
     const lanelet::CompoundPolygon3d & first_attention_area,
     const util::InterpolatedPathInfo & interpolated_path_info,
-    const std::vector<util::DescritizedLane> & lane_divisions, const double occlusion_dist_thr);
+    const std::vector<util::DescritizedLane> & lane_divisions,
+    const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> &
+      parked_attention_objects,
+    const double occlusion_dist_thr);
 
   /*
   bool IntersectionModule::checkFrontVehicleDeceleration(