Merge pull request autowarefoundation#542 from tier4/feat/improve_int…

…ersection * feat(intersection): denoise occlusion by morphology open process (autowarefoundation#3763) * apply morpholgoyEx to occlusion_mask_raw to remove noisy occlusion cell Signed-off-by: Mamoru Sobue <[email protected]> * parameterize kernel size Signed-off-by: Mamoru Sobue <[email protected]> * remove debug Signed-off-by: Mamoru Sobue <[email protected]> --------- Signed-off-by: Mamoru Sobue <[email protected]> * fix(intersection): add the flag for intersection_occlusion grid publication (autowarefoundation#3844) Signed-off-by: tomoya.kimura <[email protected]> --------- Signed-off-by: Mamoru Sobue <[email protected]> Signed-off-by: tomoya.kimura <[email protected]> Co-authored-by: Mamoru Sobue <[email protected]>
kosuke55 · May 29, 2023 · 38c74ab · 38c74ab
2 parents 0216e37 + 43c84cd
commit 38c74ab
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 22 deletions.
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -99,6 +99,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     node.declare_parameter<double>(ns + ".occlusion.min_vehicle_brake_for_rss");
   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");
 }
 
 void IntersectionModuleManager::launchNewModules(

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1100,8 +1100,14 @@ bool IntersectionModule::isOcclusionCleared(
   }
 
   // (3) occlusion mask
-  cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0));
-  cv::bitwise_and(detection_mask, unknown_mask, occlusion_mask);
+  cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0));
+  cv::bitwise_and(detection_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 / reso);
+  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 [distamce_min, distance_max]
@@ -1236,26 +1242,29 @@ bool IntersectionModule::isOcclusionCleared(
   }
   debug_data_.nearest_occlusion_point = nearest_occlusion_point;
 
-  cv::Mat distance_grid_heatmap;
-  cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
-  /*
-  cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL);
-  cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap);
-  cv::waitKey(1);
-  */
-  grid_map::GridMap occlusion_grid({"elevation"});
-  occlusion_grid.setFrameId("map");
-  occlusion_grid.setGeometry(
-    grid_map::Length(width * reso, height * reso), reso,
-    grid_map::Position(origin.x + width * reso / 2, origin.y + height * reso / 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 (planner_param_.occlusion.pub_debug_grid) {
+    cv::Mat distance_grid_heatmap;
+    cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
+    /*
+    cv::namedWindow("distance_grid_viz" + std::to_string(lane_id_), cv::WINDOW_NORMAL);
+    cv::imshow("distance_grid_viz" + std::to_string(lane_id_), distance_grid_heatmap);
+    cv::waitKey(1);
+    */
+    grid_map::GridMap occlusion_grid({"elevation"});
+    occlusion_grid.setFrameId("map");
+    occlusion_grid.setGeometry(
+      grid_map::Length(width * reso, height * reso), reso,
+      grid_map::Position(origin.x + width * reso / 2, origin.y + height * reso / 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()) {
     return true;
   } else {

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -122,6 +122,8 @@ class IntersectionModule : public SceneModuleInterface
       double before_creep_stop_time;
       double min_vehicle_brake_for_rss;
       double max_vehicle_velocity_for_rss;
+      double denoise_kernel;
+      bool pub_debug_grid;
     } occlusion;
   };
 

diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -41,6 +41,8 @@
         before_creep_stop_time: 1.0 # [s]
         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
 
     merge_from_private:
       stop_duration_sec: 1.0