Skip to content

Commit

Permalink
chore(blockage_diag): add dust diagnostic option param (#6645)
Browse files Browse the repository at this point in the history 
* fix: add dust diag option param

Signed-off-by: badai-nguyen <[email protected]>

* fix: add debug image publish option param

Signed-off-by: badai-nguyen <[email protected]>

* chore: typo

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>
  • Loading branch information
@badai-nguyen
badai-nguyen authored Mar 29, 2024
1 parent 61beca1 commit 37cd3f8
Show file tree
Hide file tree
Showing 3 changed files with 124 additions and 104 deletions.
2 changes: 2 additions & 0 deletions sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 2 additions & 0 deletions ...loud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
224 changes: 120 additions & 104 deletions sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@ 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");
publish_debug_image_ = declare_parameter<bool>("publish_debug_image");
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");
Expand All @@ -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<tier4_debug_msgs::msg::Float32Stamped>(
"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<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());
ground_dust_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"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));
Expand Down Expand Up @@ -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<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);
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_;
Expand All @@ -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<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;
}
Expand Down

0 comments on commit 37cd3f8

Please sign in to comment.