fix(intersection): add the flag for intersection_occlusion grid publi…

…cation (autowarefoundation#3844)
tier4 · May 30, 2023 · 21e5c3b · 21e5c3b
1 parent 8d511c5
commit 21e5c3b
Show file tree

Hide file tree

Showing 4 changed files with 21 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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 {