feat(intersection): extract occlusion contour as polygon (#4442)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

Author: soblin

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
+find_package(OpenCV REQUIRED)
+
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/debug.cpp
   src/manager.cpp
@@ -13,4 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/util.cpp
 )
 
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBRARIES}
+)
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -46,7 +46,7 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        pub_debug_grid: false
+        possible_object_bbox: [1.0, 2.0] # [m x m]
 
       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/package.xml

@@ -20,14 +20,9 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
-  <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
-  <depend>grid_map_cv</depend>
-  <depend>grid_map_ros</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
-  <depend>libboost-dev</depend>
-  <depend>libopencv-dev</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>

planning/behavior_velocity_intersection_module/src/debug.cpp

@@ -221,6 +221,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray(
       &debug_marker_array, now);
   }
 
+  for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) {
+    const auto & p = debug_data_.occlusion_polygons.at(j);
+    appendMarkerArray(
+      debug::createPolygonMarkerArray(
+        p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0),
+      &debug_marker_array, now);
+  }
+
   return debug_marker_array;
 }
 

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -113,7 +113,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.max_vehicle_velocity_for_rss =
     node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
-  ip.occlusion.pub_debug_grid = node.declare_parameter<bool>(ns + ".occlusion.pub_debug_grid");
+  ip.occlusion.possible_object_bbox =
+    node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
 }
 
 void IntersectionModuleManager::launchNewModules(

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -12,26 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <grid_map_cv/grid_map_cv.hpp>
-#include <grid_map_ros/grid_map_ros.hpp>
-#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
-#include <lanelet2_extension/utility/message_conversion.hpp>
-#include <lanelet2_extension/utility/query.hpp>
-#include <lanelet2_extension/utility/utilities.hpp>
-
-#if __has_include(<cv_bridge/cv_bridge.hpp>)
-#include <cv_bridge/cv_bridge.hpp>
-#else
-#include <cv_bridge/cv_bridge.h>
-#endif
-// #include <sensor_msgs/image_encodings.h>
-// #include <opencv2/highgui/highgui.hpp>
 #include "scene_intersection.hpp"
+
 #include "util.hpp"
 
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
 #include <opencv2/imgproc.hpp>
 
@@ -93,10 +84,6 @@ IntersectionModule::IntersectionModule(
     planner_param_.collision_detection.state_transit_margin_time);
   before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time);
   before_creep_state_machine_.setState(StateMachine::State::STOP);
-  if (enable_occlusion_detection) {
-    occlusion_grid_pub_ = node_.create_publisher<grid_map_msgs::msg::GridMap>(
-      "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local());
-  }
   stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
   stuck_private_area_timeout_.setState(StateMachine::State::STOP);
 }
@@ -1176,7 +1163,7 @@ bool IntersectionModule::isOcclusionCleared(
     }
   }
   for (const auto & poly : attention_area_cv_polygons) {
-    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA);
+    cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_4);
   }
   // (1.1)
   // reset adjacent_lanelets area to 0 on attention_mask
@@ -1201,8 +1188,8 @@ bool IntersectionModule::isOcclusionCleared(
     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) / resolution);
-        const int idx_y = static_cast<int>((p.y() - origin.y) / resolution);
+        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);
@@ -1231,7 +1218,7 @@ bool IntersectionModule::isOcclusionCleared(
   cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw);
   // (3.1) apply morphologyEx
   cv::Mat occlusion_mask;
-  const int morph_size = std::ceil(planner_param_.occlusion.denoise_kernel / resolution);
+  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)));
@@ -1337,59 +1324,70 @@ bool IntersectionModule::isOcclusionCleared(
     }
   }
 
-  // clean-up and find nearest risk
+  const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox;
+  const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution;
+  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);
+  std::vector<std::vector<cv::Point>> valid_contours;
+  for (const auto & contour : contours) {
+    std::vector<cv::Point> valid_contour;
+    for (const auto & p : contour) {
+      if (distance_grid.at<unsigned char>(p.y, p.x) == undef_pixel) {
+        continue;
+      }
+      valid_contour.push_back(p);
+    }
+    if (valid_contour.size() <= 2) {
+      continue;
+    }
+    std::vector<cv::Point> approx_contour;
+    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;
+    // check area
+    const double poly_area = cv::contourArea(approx_contour);
+    if (poly_area < possible_object_area) continue;
+
+    valid_contours.push_back(approx_contour);
+    geometry_msgs::msg::Polygon polygon_msg;
+    geometry_msgs::msg::Point32 point_msg;
+    for (const auto & p : approx_contour) {
+      const double glob_x = (p.x + 0.5) * resolution + origin.x;
+      const double glob_y = (height - 0.5 - p.y) * resolution + origin.y;
+      point_msg.x = glob_x;
+      point_msg.y = glob_y;
+      point_msg.z = origin.z;
+      polygon_msg.points.push_back(point_msg);
+    }
+    debug_data_.occlusion_polygons.push_back(polygon_msg);
+  }
+
   const int min_cost_thr = dist2pixel(occlusion_dist_thr);
   int min_cost = undef_pixel - 1;
-  int max_cost = 0;
-  std::optional<int> min_cost_projection_ind = std::nullopt;
   geometry_msgs::msg::Point nearest_occlusion_point;
-  for (int i = 0; i < width; ++i) {
-    for (int j = 0; j < height; ++j) {
-      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(height - 1 - j, i));
-      const bool occluded = (occlusion_mask.at<unsigned char>(height - 1 - j, i) == 255);
+  for (const auto & occlusion_contour : valid_contours) {
+    for (const auto & p : occlusion_contour) {
+      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(p.y, p.x));
+      const bool occluded = (occlusion_mask.at<unsigned char>(p.y, p.x) == 255);
       if (pixel == undef_pixel || !occluded) {
-        // ignore outside of cropped
-        // some cell maybe undef still
-        distance_grid.at<unsigned char>(height - 1 - j, i) = 0;
         continue;
       }
-      if (max_cost < pixel) {
-        max_cost = pixel;
-      }
-      const int projection_ind = projection_ind_grid.at<int>(height - 1 - j, i);
       if (pixel < min_cost) {
-        assert(projection_ind >= 0);
         min_cost = pixel;
-        min_cost_projection_ind = projection_ind;
-        nearest_occlusion_point.x = origin.x + i * resolution;
-        nearest_occlusion_point.y = origin.y + j * resolution;
-        nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel;
+        nearest_occlusion_point.x = origin.x + p.x * resolution;
+        nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution;
+        nearest_occlusion_point.z = origin.z;
       }
     }
   }
-  debug_data_.nearest_occlusion_point = nearest_occlusion_point;
-
-  if (planner_param_.occlusion.pub_debug_grid) {
-    cv::Mat distance_grid_heatmap;
-    cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
-    grid_map::GridMap occlusion_grid({"elevation"});
-    occlusion_grid.setFrameId("map");
-    occlusion_grid.setGeometry(
-      grid_map::Length(width * resolution, height * resolution), resolution,
-      grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2));
-    cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE);
-    cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE);
-    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(
-      distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */,
-      origin.z + distance_max /* elevation for 255 */);
-    grid_map::GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(
-      distance_grid_heatmap, "color", occlusion_grid);
-    occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid));
-  }
 
-  if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) {
+  if (min_cost > min_cost_thr) {
     return true;
   } else {
+    debug_data_.nearest_occlusion_point = nearest_occlusion_point;
     return false;
   }
 }
@@ -1418,9 +1416,8 @@ bool IntersectionModule::checkFrontVehicleDeceleration(
   // get the nearest centerpoint to object
   std::vector<double> dist_obj_center_points;
   for (const auto & p : center_points)
-    dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, p));
-  const int obj_closest_centerpoint_idx = std::distance(
-    dist_obj_center_points.begin(),
+    dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position,
+p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(),
     std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end()));
   // find two center_points whose distances from `closest_centerpoint` cross stopping_distance
   double acc_dist_prev = 0.0, acc_dist = 0.0;

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -20,13 +20,11 @@
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/state_machine.hpp>
-#include <grid_map_core/grid_map_core.hpp>
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <grid_map_msgs/msg/grid_map.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -108,7 +106,7 @@ class IntersectionModule : public SceneModuleInterface
       double min_vehicle_brake_for_rss;
       double max_vehicle_velocity_for_rss;
       double denoise_kernel;
-      bool pub_debug_grid;
+      std::vector<double> possible_object_bbox;
     } occlusion;
   };
 
@@ -274,7 +272,6 @@ class IntersectionModule : public SceneModuleInterface
   */
 
   util::DebugData debug_data_;
-  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr occlusion_grid_pub_;
 };
 
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -49,6 +49,7 @@ struct DebugData
   autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets;
   std::optional<geometry_msgs::msg::Point> nearest_occlusion_point = std::nullopt;
   std::optional<geometry_msgs::msg::Point> nearest_occlusion_projection_point = std::nullopt;
+  std::vector<geometry_msgs::msg::Polygon> occlusion_polygons;
 };
 
 struct InterpolatedPathInfo