From 21e5c3ba276a1a4f1afae0271ce19c00608f2288 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Fri, 26 May 2023 21:33:54 +0900
Subject: [PATCH] fix(intersection): add the flag for intersection_occlusion
 grid publication (#3844)

---
 .../config/intersection.param.yaml            |  1 +
 .../intersection/scene_intersection.hpp       |  1 +
 .../src/scene_module/intersection/manager.cpp |  1 +
 .../intersection/scene_intersection.cpp       | 38 +++++++++----------
 4 files changed, 21 insertions(+), 20 deletions(-)

diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
index 3f019f2e1ecf7..0fdb9969e432b 100644
--- a/planning/behavior_velocity_planner/config/intersection.param.yaml
+++ b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -41,6 +41,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
 
     merge_from_private:
       stop_duration_sec: 1.0
diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
index 6a2047a6d9d10..ef56aaf21c30e 100644
--- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
+++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp
@@ -122,6 +122,7 @@ class IntersectionModule : public SceneModuleInterface
       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/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
index b5758f7f6c128..0f97883ed0a98 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
@@ -96,6 +96,7 @@ 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");
 }
 
 void IntersectionModuleManager::launchNewModules(
diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
index 07a7ad09bfade..cabb1d00cdfcd 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -1255,26 +1255,24 @@ 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);
+    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()) {
     return true;
   } else {