From e756f433d8e286c2b9f80add99b167115567025d Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 18 Mar 2024 12:48:52 +0900 Subject: [PATCH 1/3] fix: add dust diag option param Signed-off-by: badai-nguyen --- .../blockage_diagnostics_param_file.yaml | 1 + .../blockage_diag/blockage_diag_nodelet.hpp | 1 + .../blockage_diag/blockage_diag_nodelet.cpp | 202 +++++++++--------- 3 files changed, 106 insertions(+), 98 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..1fed27f662c82 100644 --- a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/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 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..aa01d0fbfebcf 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,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_; 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..57e0bdb99248d 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,7 @@ 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"); + 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,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( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); sky_blockage_ratio_pub_ = create_publisher( "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( "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(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(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; } From 6d8606f268002c23cf1d637c3599aa783d516be4 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 18 Mar 2024 15:08:08 +0900 Subject: [PATCH 2/3] fix: add debug image publish option param Signed-off-by: badai-nguyen --- .../blockage_diagnostics_param_file.yaml | 1 + .../blockage_diag/blockage_diag_nodelet.hpp | 1 + .../blockage_diag/blockage_diag_nodelet.cpp | 173 +++++++++--------- 3 files changed, 93 insertions(+), 82 deletions(-) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml index 1fed27f662c82..13ddd8a7c7590 100644 --- a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -5,6 +5,7 @@ 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 aa01d0fbfebcf..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 @@ -86,6 +86,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter 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 57e0bdb99248d..077e50c93b835 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,7 @@ 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"); @@ -64,25 +65,28 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options updater_.add( std::string(this->get_namespace()) + ": dust_validation", this, &BlockageDiagComponent::dustChecker); + 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"); + ground_dust_ratio_pub_ = create_publisher( + "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); + } } updater_.setPeriod(0.1); - 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"); + 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()); - - 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( - "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)); @@ -305,67 +309,13 @@ 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); - 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); - 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(); + 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_++; @@ -373,16 +323,62 @@ void BlockageDiagComponent::filter( } 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(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 (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); + } } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; @@ -394,6 +390,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); + 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); + } + pcl::toROSMsg(*pcl_input, output); output.header = input->header; } From 367c47051566c284de79ea39cff5e73280369b2c Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 21 Mar 2024 19:13:56 +0900 Subject: [PATCH 3/3] chore: typo Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 077e50c93b835..773e6db056990 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -65,6 +65,9 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options 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"); @@ -72,8 +75,6 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options 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( - "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); } } updater_.setPeriod(0.1);