From ce8758eb4184a2d1551cd6cb4cf839f98159ea48 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 27 Feb 2024 21:54:37 +0900 Subject: [PATCH 1/2] refactor(pointcloud_preprocessor): blockage_diag (#5932) * chore(blockage_diag): refactor Signed-off-by: badai-nguyen * chore: add error logger Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * fix: break interval angle as continuous one Signed-off-by: badai-nguyen * chore: update param file Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: multi-blockage-diag in 1 container died bug fix Signed-off-by: badai-nguyen * chore: update param_file Signed-off-by: badai-nguyen * fix: Nan sky_blockage_ratio Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../pointcloud_preprocessor/CMakeLists.txt | 1 + .../blockage_diagnostics_param_file.yaml | 14 ++++ .../docs/blockage_diag.md | 5 +- .../blockage_diag/blockage_diag_nodelet.hpp | 8 +- .../launch/blockage_diag.launch.xml | 27 +----- .../blockage_diag/blockage_diag_nodelet.cpp | 82 ++++++++++--------- 6 files changed, 73 insertions(+), 64 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index c5f15ec4e49d5..219f43e48730a 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -211,6 +211,7 @@ ament_export_targets(export_${PROJECT_NAME}) ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 0000000000000..c932e7f62d5bc --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + max_distance_range: 200.0 + horizontal_resolution: 0.4 + blockage_kernel: 10 diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 32e5a5869109d..082ae180fa3c9 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -58,14 +58,17 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | | `angle_range` | vector | The effective range of LiDAR | | `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | +| `is_channel_order_top2down` | bool | If the lidar channels are indexed from top to down | | `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | | `blockage_buffering_interval` | int | The interval of buffering about blockage detection | | `dust_ratio_threshold` | float | The threshold of dusty area ratio | | `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `blockage_kernel` | int | The kernel size of morphology processing the detected blockage area | | `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | | `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | | `dust_buffering_interval` | int | The interval of buffering about dusty area detection | +| `max_distance_range` | double | Maximum view range for the LiDAR | +| `horizontal_resolution` | double | The horizontal resolution of depth map image [deg/pixel] | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 66e14c418b52c..c1cf1e59f0343 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -33,6 +33,8 @@ #include #endif +#include + #include #include @@ -80,7 +82,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int ground_blockage_count_ = 0; int sky_blockage_count_ = 0; int blockage_count_threshold_; - std::string lidar_model_; + bool is_channel_order_top2down_; int blockage_buffering_frames_; int blockage_buffering_interval_; int dust_kernel_size_; @@ -89,6 +91,10 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int dust_buffering_frame_counter_ = 0; int dust_count_threshold_; int dust_frame_count_ = 0; + double max_distance_range_{200.0}; + double horizontal_resolution_{0.4}; + boost::circular_buffer no_return_mask_buffer{1}; + boost::circular_buffer dust_mask_buffer{1}; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 8fa5796e2802d..5a93cb950b827 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -1,37 +1,18 @@ - - - - - - - - - - - - + + - - - - - - - - - - - + + diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 0dd7088f80262..17085ec0a3120 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -16,8 +16,6 @@ #include "autoware_point_types/types.hpp" -#include - #include #include @@ -35,7 +33,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); vertical_bins_ = declare_parameter("vertical_bins"); angle_range_deg_ = declare_parameter>("angle_range"); - lidar_model_ = declare_parameter("model"); + is_channel_order_top2down_ = declare_parameter("is_channel_order_top2down"); blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); @@ -44,6 +42,17 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options dust_kernel_size_ = declare_parameter("dust_kernel_size"); dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); + max_distance_range_ = declare_parameter("max_distance_range"); + horizontal_resolution_ = declare_parameter("horizontal_resolution"); + blockage_kernel_ = declare_parameter("blockage_kernel"); + } + dust_mask_buffer.set_capacity(dust_buffering_frames_); + no_return_mask_buffer.set_capacity(blockage_buffering_frames_); + if (vertical_bins_ <= horizontal_ring_id_) { + RCLCPP_ERROR( + this->get_logger(), + "The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"); + return; } updater_.setHardwareID("blockage_diag"); @@ -150,24 +159,17 @@ void BlockageDiagComponent::filter( std::scoped_lock lock(mutex_); int vertical_bins = vertical_bins_; int ideal_horizontal_bins; - float distance_coefficient = 327.67f; - float horizontal_resolution_ = 0.4f; - if (lidar_model_ == "Pandar40P") { - distance_coefficient = 327.67f; - horizontal_resolution_ = 0.4f; - } else if (lidar_model_ == "PandarQT") { - distance_coefficient = 3276.75f; - horizontal_resolution_ = 0.6f; + double compensate_angle = 0.0; + // Check the case when angle_range_deg_[1] exceed 360 and shifted the range to 0~360 + if (angle_range_deg_[0] > angle_range_deg_[1]) { + compensate_angle = 360.0; } - ideal_horizontal_bins = - static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); + ideal_horizontal_bins = static_cast( + (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - std::vector horizontal_bin_reference(ideal_horizontal_bins); - std::vector> each_ring_pointcloud(vertical_bins); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); cv::Mat lidar_depth_map_8u( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { @@ -184,24 +186,23 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (int i = 0; i < ideal_horizontal_bins; ++i) { - horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; - } for (const auto p : pcl_input->points) { - for (int horizontal_bin = 0; - horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { - if ( - (p.azimuth / 100 > - (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && - (p.azimuth / 100 <= - (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { - if (lidar_model_ == "Pandar40P") { - full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } + double azimuth_deg = p.azimuth / 100.; + if ( + ((azimuth_deg > angle_range_deg_[0]) && + (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || + ((azimuth_deg + compensate_angle > angle_range_deg_[0]) && + (azimuth_deg < angle_range_deg_[1]))) { + double current_angle_range = (azimuth_deg + compensate_angle - angle_range_deg_[0]); + int horizontal_bin_index = static_cast(current_angle_range / horizontal_resolution_) % + static_cast(360.0 / horizontal_resolution_); + uint16_t depth_intensity = + UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); + if (is_channel_order_top2down_) { + full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + } else { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + depth_intensity; } } } @@ -215,7 +216,6 @@ void BlockageDiagComponent::filter( cv::Point(blockage_kernel_, blockage_kernel_)); cv::erode(no_return_mask, erosion_dst, blockage_element); cv::dilate(erosion_dst, no_return_mask, blockage_element); - static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); cv::Mat time_series_blockage_result( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat time_series_blockage_mask( @@ -250,8 +250,13 @@ void BlockageDiagComponent::filter( ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(ideal_horizontal_bins * horizontal_ring_id_); + + if (horizontal_ring_id_ == 0) { + sky_blockage_ratio_ = 0.0f; + } else { + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); + } if (ground_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); @@ -295,7 +300,6 @@ void BlockageDiagComponent::filter( 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); - static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); cv::Mat binarized_dust_mask_( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat multi_frame_dust_mask( @@ -405,8 +409,8 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( RCLCPP_DEBUG( get_logger(), "Setting new blockage_count_threshold to: %d.", blockage_count_threshold_); } - if (get_param(p, "model", lidar_model_)) { - RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %s. ", lidar_model_.c_str()); + if (get_param(p, "is_channel_order_top2down", is_channel_order_top2down_)) { + RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %d. ", is_channel_order_top2down_); } if (get_param(p, "angle_range", angle_range_deg_)) { RCLCPP_DEBUG( From afff942ca2e1d23396fb8c5d1675f837de64eb84 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 29 Mar 2024 15:21:23 +0900 Subject: [PATCH 2/2] chore(blockage_diag): add dust diagnostic option param (#6645) * fix: add dust diag option param Signed-off-by: badai-nguyen * fix: add debug image publish option param Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../blockage_diagnostics_param_file.yaml | 2 + .../blockage_diag/blockage_diag_nodelet.hpp | 2 + .../blockage_diag/blockage_diag_nodelet.cpp | 224 ++++++++++-------- 3 files changed, 124 insertions(+), 104 deletions(-) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml index c932e7f62d5bc..13ddd8a7c7590 100644 --- a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -4,6 +4,8 @@ blockage_count_threshold: 50 blockage_buffering_frames: 2 blockage_buffering_interval: 1 + enable_dust_diag: false + publish_debug_image: false dust_ratio_threshold: 0.2 dust_count_threshold: 10 dust_kernel_size: 2 diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index c1cf1e59f0343..dd24c83761878 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -85,6 +85,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter bool is_channel_order_top2down_; int blockage_buffering_frames_; int blockage_buffering_interval_; + bool enable_dust_diag_; + bool publish_debug_image_; int dust_kernel_size_; int dust_buffering_frames_; int dust_buffering_interval_; diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 17085ec0a3120..773e6db056990 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -37,6 +37,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); + publish_debug_image_ = declare_parameter("publish_debug_image"); + enable_dust_diag_ = declare_parameter("enable_dust_diag"); dust_ratio_threshold_ = declare_parameter("dust_ratio_threshold"); dust_count_threshold_ = declare_parameter("dust_count_threshold"); dust_kernel_size_ = declare_parameter("dust_kernel_size"); @@ -59,26 +61,33 @@ 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); + + ground_dust_ratio_pub_ = create_publisher( + "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); + if (publish_debug_image_) { + 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"); + } + } 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"); + if (publish_debug_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"); + } ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); - ground_dust_ratio_pub_ = create_publisher( - "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( std::bind(&BlockageDiagComponent::paramCallback, this, _1)); @@ -283,93 +292,95 @@ 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); - 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_); - dust_buffering_frame_counter_ = 0; + tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + ground_dust_ratio_ = static_cast(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); + if (ground_dust_ratio_ > dust_ratio_threshold_) { + if (dust_frame_count_ < 2 * dust_count_threshold_) { + dust_frame_count_++; + } } else { - dust_buffering_frame_counter_++; - } - for (const auto & binarized_dust_mask : dust_mask_buffer) { - multi_frame_dust_mask += binarized_dust_mask; + dust_frame_count_ = 0; } - 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_++; + + if (publish_debug_image_) { + 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_); + 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::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); + 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(); + blockage_dust_merged_msg->header = input->header; + blockage_dust_merged_pub.publish(blockage_dust_merged_msg); } - } 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 ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; @@ -380,14 +391,19 @@ 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; + if (publish_debug_image_) { + 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); + } - ground_dust_ratio_ = static_cast(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; }