diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index 6a1590b4197ce..c476b6de12332 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -100,6 +100,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_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index 2b8989478f21f..5a46b24a03433 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1256,26 +1256,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 * 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 (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 {
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
index 2a9e6dc678005..778d81f69e91a 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -123,6 +123,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/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml
index d10d1093fc789..c86e95ae7e168 100644
--- a/planning/behavior_velocity_planner/config/intersection.param.yaml
+++ b/planning/behavior_velocity_planner/config/intersection.param.yaml
@@ -42,6 +42,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