fix: add dust diag option param

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml

@@ -4,6 +4,7 @@
     blockage_count_threshold: 50
     blockage_buffering_frames: 2
     blockage_buffering_interval: 1
+    enable_dust_diag: false
     dust_ratio_threshold: 0.2
     dust_count_threshold: 10
     dust_kernel_size: 2

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp

@@ -85,6 +85,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter
   bool is_channel_order_top2down_;
   int blockage_buffering_frames_;
   int blockage_buffering_interval_;
+  bool enable_dust_diag_;
   int dust_kernel_size_;
   int dust_buffering_frames_;
   int dust_buffering_interval_;

sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

@@ -37,6 +37,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
     blockage_count_threshold_ = declare_parameter<int>("blockage_count_threshold");
     blockage_buffering_frames_ = declare_parameter<int>("blockage_buffering_frames");
     blockage_buffering_interval_ = declare_parameter<int>("blockage_buffering_interval");
+    enable_dust_diag_ = declare_parameter<bool>("enable_dust_diag");
     dust_ratio_threshold_ = declare_parameter<float>("dust_ratio_threshold");
     dust_count_threshold_ = declare_parameter<int>("dust_count_threshold");
     dust_kernel_size_ = declare_parameter<int>("dust_kernel_size");
@@ -59,24 +60,27 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
   updater_.add(
     std::string(this->get_namespace()) + ": blockage_validation", this,
     &BlockageDiagComponent::onBlockageChecker);
-  updater_.add(
-    std::string(this->get_namespace()) + ": dust_validation", this,
-    &BlockageDiagComponent::dustChecker);
+  if (enable_dust_diag_) {
+    updater_.add(
+      std::string(this->get_namespace()) + ": dust_validation", this,
+      &BlockageDiagComponent::dustChecker);
+  }
   updater_.setPeriod(0.1);
-  single_frame_dust_mask_pub =
-    image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image");
-  multi_frame_dust_mask_pub =
-    image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image");
   lidar_depth_map_pub_ =
     image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map");
   blockage_mask_pub_ =
     image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image");
-  blockage_dust_merged_pub =
-    image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image");
   ground_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
     "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS());
   sky_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
     "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS());
+
+  single_frame_dust_mask_pub =
+    image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image");
+  multi_frame_dust_mask_pub =
+    image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image");
+  blockage_dust_merged_pub =
+    image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image");
   ground_dust_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
     "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS());
   using std::placeholders::_1;
@@ -283,93 +287,103 @@ void BlockageDiagComponent::filter(
     sky_blockage_count_ = 0;
   }
   // dust
-  cv::Mat ground_depth_map = lidar_depth_map_8u(
-    cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_));
-  cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
-  cv::Mat ground_blank(
-    vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
-  cv::Mat single_dust_img(
-    cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0));
-  cv::Mat single_dust_ground_img = ground_depth_map.clone();
-  cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img);
-  cv::Mat dust_element = getStructuringElement(
-    cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1),
-    cv::Point(-1, -1));
-  cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element);
-  cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element);
-  cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img);
-  cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1);
-  cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img);
-  cv::Mat binarized_dust_mask_(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
-  cv::Mat multi_frame_dust_mask(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
-  cv::Mat multi_frame_ground_dust_result(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
+  if (enable_dust_diag_) {
+    cv::Mat ground_depth_map = lidar_depth_map_8u(
+      cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_));
+    cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
+    cv::Mat ground_blank(
+      vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
+    cv::Mat single_dust_img(
+      cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0));
+    cv::Mat single_dust_ground_img = ground_depth_map.clone();
+    cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img);
+    cv::Mat dust_element = getStructuringElement(
+      cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1),
+      cv::Point(-1, -1));
+    cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element);
+    cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element);
+    cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img);
+    cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1);
+    cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img);
+    cv::Mat binarized_dust_mask_(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
+    cv::Mat multi_frame_dust_mask(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
+    cv::Mat multi_frame_ground_dust_result(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
 
-  if (dust_buffering_interval_ == 0) {
-    single_dust_img.copyTo(multi_frame_ground_dust_result);
-    dust_buffering_frame_counter_ = 0;
-  } else {
-    binarized_dust_mask_ = single_dust_img / 255;
-    if (dust_buffering_frame_counter_ >= dust_buffering_interval_) {
-      dust_mask_buffer.push_back(binarized_dust_mask_);
+    if (dust_buffering_interval_ == 0) {
+      single_dust_img.copyTo(multi_frame_ground_dust_result);
       dust_buffering_frame_counter_ = 0;
     } else {
-      dust_buffering_frame_counter_++;
-    }
-    for (const auto & binarized_dust_mask : dust_mask_buffer) {
-      multi_frame_dust_mask += binarized_dust_mask;
+      binarized_dust_mask_ = single_dust_img / 255;
+      if (dust_buffering_frame_counter_ >= dust_buffering_interval_) {
+        dust_mask_buffer.push_back(binarized_dust_mask_);
+        dust_buffering_frame_counter_ = 0;
+      } else {
+        dust_buffering_frame_counter_++;
+      }
+      for (const auto & binarized_dust_mask : dust_mask_buffer) {
+        multi_frame_dust_mask += binarized_dust_mask;
+      }
+      cv::inRange(
+        multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(),
+        multi_frame_ground_dust_result);
     }
-    cv::inRange(
-      multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(),
-      multi_frame_ground_dust_result);
-  }
-  cv::Mat single_frame_ground_dust_colorized(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
-  cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET);
-  cv::Mat multi_frame_ground_dust_colorized;
-  cv::Mat blockage_dust_merged_img(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
-  cv::Mat blockage_dust_merged_mask(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
-  blockage_dust_merged_img.setTo(
-    cv::Vec3b(0, 0, 255), time_series_blockage_result);  // red:blockage
-  blockage_dust_merged_img.setTo(
-    cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result);  // yellow:dust
-  sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg =
-    cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized)
-      .toImageMsg();
-  single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg);
-  sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg =
-    cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized)
-      .toImageMsg();
-  multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg);
-  sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg =
-    cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg();
-  lidar_depth_map_msg->header = input->header;
-  lidar_depth_map_pub_.publish(lidar_depth_map_msg);
-  cv::Mat blockage_mask_colorized;
-  cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET);
-  sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
-    cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
-  blockage_mask_msg->header = input->header;
-  blockage_mask_pub_.publish(blockage_mask_msg);
-  cv::Mat blockage_dust_merged_colorized(
-    cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
-  blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized);
-  sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg =
-    cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized)
-      .toImageMsg();
-  if (ground_dust_ratio_ > dust_ratio_threshold_) {
-    if (dust_frame_count_ < 2 * dust_count_threshold_) {
-      dust_frame_count_++;
+    cv::Mat single_frame_ground_dust_colorized(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
+    cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET);
+    cv::Mat multi_frame_ground_dust_colorized;
+    cv::Mat blockage_dust_merged_img(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
+    cv::Mat blockage_dust_merged_mask(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
+    blockage_dust_merged_img.setTo(
+      cv::Vec3b(0, 0, 255), time_series_blockage_result);  // red:blockage
+    blockage_dust_merged_img.setTo(
+      cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result);  // yellow:dust
+    sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized)
+        .toImageMsg();
+    single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg);
+    sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized)
+        .toImageMsg();
+    multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg);
+    sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg();
+    lidar_depth_map_msg->header = input->header;
+    lidar_depth_map_pub_.publish(lidar_depth_map_msg);
+    cv::Mat blockage_mask_colorized;
+    cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET);
+    sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
+    blockage_mask_msg->header = input->header;
+    blockage_mask_pub_.publish(blockage_mask_msg);
+    cv::Mat blockage_dust_merged_colorized(
+      cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
+    blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized);
+    sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg =
+      cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized)
+        .toImageMsg();
+    if (ground_dust_ratio_ > dust_ratio_threshold_) {
+      if (dust_frame_count_ < 2 * dust_count_threshold_) {
+        dust_frame_count_++;
+      }
+    } else {
+      dust_frame_count_ = 0;
     }
-  } else {
-    dust_frame_count_ = 0;
+    blockage_dust_merged_msg->header = input->header;
+    blockage_dust_merged_pub.publish(blockage_dust_merged_msg);
+    tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg;
+    tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg;
+
+    ground_dust_ratio_ = static_cast<float>(cv::countNonZero(single_dust_ground_img)) /
+                         (single_dust_ground_img.cols * single_dust_ground_img.rows);
+    ground_dust_ratio_msg.data = ground_dust_ratio_;
+    ground_dust_ratio_msg.stamp = now();
+    ground_dust_ratio_pub_->publish(ground_dust_ratio_msg);
   }
-  blockage_dust_merged_msg->header = input->header;
-  blockage_dust_merged_pub.publish(blockage_dust_merged_msg);
 
   tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg;
   ground_blockage_ratio_msg.data = ground_blockage_ratio_;
@@ -380,14 +394,6 @@ void BlockageDiagComponent::filter(
   sky_blockage_ratio_msg.data = sky_blockage_ratio_;
   sky_blockage_ratio_msg.stamp = now();
   sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg);
-  tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg;
-  tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg;
-
-  ground_dust_ratio_ = static_cast<float>(cv::countNonZero(single_dust_ground_img)) /
-                       (single_dust_ground_img.cols * single_dust_ground_img.rows);
-  ground_dust_ratio_msg.data = ground_dust_ratio_;
-  ground_dust_ratio_msg.stamp = now();
-  ground_dust_ratio_pub_->publish(ground_dust_ratio_msg);
   pcl::toROSMsg(*pcl_input, output);
   output.header = input->header;
 }