From ac7ca5e9f124a11e5f3afc9209bcc7bf1a570d2f Mon Sep 17 00:00:00 2001 From: Yusuke Mizoguchi <33048729+swiftfile@users.noreply.github.com> Date: Fri, 29 Sep 2023 01:46:49 +0900 Subject: [PATCH] feat(blockage_diag): dust detection (#3212) * implement about dust detection Signed-off-by: Yusuke Mizoguchi * chore: fix spell Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * bug fix Signed-off-by: Shunsuke Miura * fix: avoid counter overflow Signed-off-by: badai-nguyen --------- Signed-off-by: Yusuke Mizoguchi Signed-off-by: badai-nguyen Signed-off-by: Shunsuke Miura Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: badai-nguyen Co-authored-by: Shunsuke Miura --- .../docs/blockage_diag.md | 63 ++- .../blockage_diag/blockage_diag_nodelet.hpp | 31 +- .../launch/blockage_diag.launch.xml | 28 ++ .../blockage_diag/blockage_diag_nodelet.cpp | 389 ++++++++++++------ 4 files changed, 362 insertions(+), 149 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 519bd831fd39c..32e5a5869109d 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -2,11 +2,13 @@ ## Purpose -To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is needed. -LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return signal. +To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is +needed. +LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return +signal. This node's purpose is to detect the existing of blockage on LiDAR and its related size and location. -## Inner-workings / Algorithms +## Inner-workings / Algorithms(Blockage detection) This node bases on the no-return region and its location to decide if it is a blockage. @@ -16,6 +18,14 @@ The logic is showed as below ![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg) +## Inner-workings /Algorithms(Dust detection) + +About dust detection, morphological processing is implemented. +If the lidar's ray cannot be acquired due to dust in the lidar area where the point cloud is considered to return from +the ground, +black pixels appear as noise in the depth image. +The area of noise is found by erosion and dilation these black pixels. + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -28,30 +38,41 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Output -| Name | Type | Description | -| ---------------------------------------------------- | --------------------------------------- | -------------------------------------------------- | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters -| Name | Type | Description | -| -------------------------- | ------ | -------------------------------------------------- | -| `blockage_ratio_threshold` | float | The threshold of blockage area ratio | -| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | -| `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 | -| `buffering_frames` | uint | The number of buffering [range:1-200] | -| `buffering_interval` | uint | The interval of buffering | +| Name | Type | Description | +| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- | +| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. | +| `blockage_count_threshold` | float | The threshold of number continuous blockage frames | +| `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 | +| `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 | +| `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 | ## Assumptions / Known limits -1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code. -2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed. +1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in + vertical distribution manually and modify the code. +2. About dusty area detection, False positives occur when there are water puddles on the road surface due to rain, etc. + Also, the area of the ray to the sky is currently undetectable. ## (Optional) Error detection and handling 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 116dd2f832b93..66e14c418b52c 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 @@ -53,27 +53,42 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; + image_transport::Publisher single_frame_dust_mask_pub; + image_transport::Publisher multi_frame_dust_mask_pub; + image_transport::Publisher blockage_dust_merged_pub; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); + void dustChecker(DiagnosticStatusWrapper & stat); Updater updater_{this}; - uint vertical_bins_; + int vertical_bins_; std::vector angle_range_deg_; - uint horizontal_ring_id_ = 12; + int horizontal_ring_id_; float blockage_ratio_threshold_; + float dust_ratio_threshold_; float ground_blockage_ratio_ = -1.0f; float sky_blockage_ratio_ = -1.0f; + float ground_dust_ratio_ = -1.0f; std::vector ground_blockage_range_deg_ = {0.0f, 0.0f}; std::vector sky_blockage_range_deg_ = {0.0f, 0.0f}; - uint erode_kernel_ = 10; - uint ground_blockage_count_ = 0; - uint sky_blockage_count_ = 0; - uint blockage_count_threshold_; + int blockage_kernel_ = 10; + int blockage_frame_count_ = 0; + int ground_blockage_count_ = 0; + int sky_blockage_count_ = 0; + int blockage_count_threshold_; std::string lidar_model_; - uint buffering_frames_ = 100; - uint buffering_interval_ = 5; + int blockage_buffering_frames_; + int blockage_buffering_interval_; + int dust_kernel_size_; + int dust_buffering_frames_; + int dust_buffering_interval_; + int dust_buffering_frame_counter_ = 0; + int dust_count_threshold_; + int dust_frame_count_ = 0; 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 47dc1e73eef12..8fa5796e2802d 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -2,8 +2,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 3563bf2b6c408..06da91e61b3bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -19,6 +19,7 @@ #include #include +#include namespace pointcloud_preprocessor { @@ -30,34 +31,45 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options { { // initialize params: - horizontal_ring_id_ = static_cast(declare_parameter("horizontal_ring_id", 12)); - blockage_ratio_threshold_ = - static_cast(declare_parameter("blockage_ratio_threshold", 0.1)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 40)); - angle_range_deg_ = declare_parameter("angle_range", std::vector{0.0, 360.0}); - lidar_model_ = static_cast(declare_parameter("model", "Pandar40P")); - blockage_count_threshold_ = - static_cast(declare_parameter("blockage_count_threshold", 50)); - buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); - buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); + horizontal_ring_id_ = declare_parameter("horizontal_ring_id"); + 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"); + blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); + blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); + blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); + dust_ratio_threshold_ = declare_parameter("dust_ratio_threshold"); + dust_count_threshold_ = declare_parameter("dust_count_threshold"); + dust_kernel_size_ = declare_parameter("dust_kernel_size"); + dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); + dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); } updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, &BlockageDiagComponent::onBlockageChecker); + 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( "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)); @@ -105,19 +117,59 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.summary(level, msg); } +void BlockageDiagComponent::dustChecker(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + stat.add("ground_dust_ratio", std::to_string(ground_dust_ratio_)); + auto level = DiagnosticStatus::OK; + std::string msg; + if (ground_dust_ratio_ < 0.0f) { + level = DiagnosticStatus::STALE; + msg = "STALE"; + } else if ( + (ground_dust_ratio_ > dust_ratio_threshold_) && (dust_frame_count_ > dust_count_threshold_)) { + level = DiagnosticStatus::ERROR; + msg = "ERROR"; + } else if (ground_dust_ratio_ > 0.0f) { + level = DiagnosticStatus::WARN; + msg = "WARN"; + } else { + level = DiagnosticStatus::OK; + msg = "OK"; + } + + if (ground_dust_ratio_ > 0.0f) { + msg = msg + ": LIDAR ground dust"; + } + stat.summary(level, msg); +} + void BlockageDiagComponent::filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) { std::scoped_lock lock(mutex_); - if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + 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; } - uint horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0])); - uint vertical_bins = vertical_bins_; + ideal_horizontal_bins = + static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - cv::Mat lidar_depth_map(cv::Size(horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map_8u(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + 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()) { ground_blockage_ratio_ = 1.0f; sky_blockage_ratio_ = 1.0f; @@ -132,114 +184,186 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (const auto & p : pcl_input->points) { - if ((p.azimuth / 100.0 > angle_range_deg_[0]) && (p.azimuth / 100.0 < angle_range_deg_[1])) { - if (lidar_model_ == "Pandar40P") { - lidar_depth_map.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); // make image clearly - lidar_depth_map_8u.at( - p.ring, static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; - } else if (lidar_model_ == "PandarQT") { - lidar_depth_map.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) += - static_cast(6250.0 / p.distance); - lidar_depth_map_8u.at( - vertical_bins - p.ring - 1, - static_cast((p.azimuth / 100.0 - angle_range_deg_[0]))) = 255; + 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; + } } } } - lidar_depth_map.convertTo(lidar_depth_map, CV_8UC1, 1.0 / 100.0); - cv::Mat no_return_mask; - cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); - - cv::Mat erosion_dst; - cv::Mat element = cv::getStructuringElement( - cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), - cv::Point(erode_kernel_, erode_kernel_)); - cv::erode(no_return_mask, erosion_dst, element); - cv::dilate(erosion_dst, no_return_mask, element); - - static boost::circular_buffer no_return_mask_buffer(buffering_frames_); - - cv::Mat no_return_mask_result(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat time_series_blockage_mask( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::Mat no_return_mask_binarized( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + } + full_size_depth_map.convertTo(lidar_depth_map_8u, CV_8UC1, 1.0 / 300); + cv::Mat no_return_mask(cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::inRange(lidar_depth_map_8u, 0, 1, no_return_mask); + cv::Mat erosion_dst; + cv::Mat blockage_element = cv::getStructuringElement( + cv::MORPH_RECT, cv::Size(2 * blockage_kernel_ + 1, 2 * blockage_kernel_ + 1), + 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( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binarized( + cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - static uint frame_count; - frame_count++; - if (buffering_interval_ != 0) { - no_return_mask_binarized = no_return_mask / 255; - if (frame_count == buffering_interval_) { - no_return_mask_buffer.push_back(no_return_mask_binarized); - frame_count = 0; - } - for (const auto & binary_mask : no_return_mask_buffer) { - time_series_blockage_mask += binary_mask; - } - cv::inRange( - time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), - no_return_mask_result); + if (blockage_buffering_interval_ == 0) { + no_return_mask.copyTo(time_series_blockage_result); + } else { + no_return_mask_binarized = no_return_mask / 255; + if (blockage_frame_count_ >= blockage_buffering_interval_) { + no_return_mask_buffer.push_back(no_return_mask_binarized); + blockage_frame_count_ = 0; } else { - no_return_mask.copyTo(no_return_mask_result); + blockage_frame_count_++; } - cv::Mat ground_no_return_mask; - cv::Mat sky_no_return_mask; - no_return_mask_result(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)) - .copyTo(sky_no_return_mask); - no_return_mask_result( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); - ground_blockage_ratio_ = - static_cast(cv::countNonZero(ground_no_return_mask)) / - static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(horizontal_bins * horizontal_ring_id_); - - if (ground_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); - ground_blockage_range_deg_[0] = - static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; - ground_blockage_range_deg_[1] = - static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + for (const auto & binary_mask : no_return_mask_buffer) { + time_series_blockage_mask += binary_mask; + } + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + time_series_blockage_result); + } + cv::Mat ground_no_return_mask; + cv::Mat sky_no_return_mask; + no_return_mask(cv::Rect(0, 0, ideal_horizontal_bins, horizontal_ring_id_)) + .copyTo(sky_no_return_mask); + no_return_mask( + cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_)) + .copyTo(ground_no_return_mask); + 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 (ground_blockage_count_ <= 2 * blockage_count_threshold_) { - ground_blockage_count_ += 1; - } - } else { - ground_blockage_count_ = 0; + if (ground_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); + ground_blockage_range_deg_[0] = static_cast(ground_blockage_bb.x) + angle_range_deg_[0]; + ground_blockage_range_deg_[1] = + static_cast(ground_blockage_bb.x + ground_blockage_bb.width) + angle_range_deg_[0]; + if (ground_blockage_count_ <= 2 * blockage_count_threshold_) { + ground_blockage_count_ += 1; } + } else { + ground_blockage_count_ = 0; + } + if (sky_blockage_ratio_ > blockage_ratio_threshold_) { + cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); + sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; + sky_blockage_range_deg_[1] = + static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; + if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { + sky_blockage_count_ += 1; + } + } else { + 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); + 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( + 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 (sky_blockage_ratio_ > blockage_ratio_threshold_) { - cv::Rect sky_blockage_bx = cv::boundingRect(sky_no_return_mask); - sky_blockage_range_deg_[0] = static_cast(sky_blockage_bx.x) + angle_range_deg_[0]; - sky_blockage_range_deg_[1] = - static_cast(sky_blockage_bx.x + sky_blockage_bx.width) + angle_range_deg_[0]; - if (sky_blockage_count_ <= 2 * blockage_count_threshold_) { - sky_blockage_count_ += 1; - } + 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 { - sky_blockage_count_ = 0; + dust_buffering_frame_counter_++; } - - cv::Mat lidar_depth_colorized; - cv::applyColorMap(lidar_depth_map, lidar_depth_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr lidar_depth_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", lidar_depth_colorized).toImageMsg(); - lidar_depth_msg->header = input->header; - lidar_depth_map_pub_.publish(lidar_depth_msg); - - cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask_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); + 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); + 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; } + 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_; @@ -250,7 +374,14 @@ 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(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; } @@ -280,11 +411,29 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), " Setting new angle_range to: [%f , %f].", angle_range_deg_[0], angle_range_deg_[1]); } - if (get_param(p, "buffering_frames", buffering_frames_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); + if (get_param(p, "blockage_buffering_frames", blockage_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_frames_ to: %d.", blockage_buffering_frames_); + } + if (get_param(p, "blockage_buffering_interval", blockage_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new blockage_buffering_interval_ to: %d.", + blockage_buffering_interval_); + } + if (get_param(p, "dust_kernel_size", dust_kernel_size_)) { + RCLCPP_DEBUG(get_logger(), "Setting new dust_kernel_size_ to: %d.", dust_kernel_size_); } - if (get_param(p, "buffering_interval", buffering_interval_)) { - RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + if (get_param(p, "dust_buffering_frames", dust_buffering_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_frames_ to: %d.", dust_buffering_frames_); + // note:NOT affects to actual variable. + // if you want change this param/variable, change the parameter called at launch this + // node(aip_launcher). + } + if (get_param(p, "dust_buffering_interval", dust_buffering_interval_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new dust_buffering_interval_ to: %d.", dust_buffering_interval_); + dust_buffering_frame_counter_ = 0; } rcl_interfaces::msg::SetParametersResult result; result.successful = true;