Skip to content

Commit e756f43

Browse files
committed
fix: add dust diag option param
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent eea40ae commit e756f43

File tree

3 files changed

+106
-98
lines changed

3 files changed

+106
-98
lines changed

sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml

+1
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
blockage_count_threshold: 50
55
blockage_buffering_frames: 2
66
blockage_buffering_interval: 1
7+
enable_dust_diag: false
78
dust_ratio_threshold: 0.2
89
dust_count_threshold: 10
910
dust_kernel_size: 2

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

+1
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter
8585
bool is_channel_order_top2down_;
8686
int blockage_buffering_frames_;
8787
int blockage_buffering_interval_;
88+
bool enable_dust_diag_;
8889
int dust_kernel_size_;
8990
int dust_buffering_frames_;
9091
int dust_buffering_interval_;

sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp

+104-98
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
3737
blockage_count_threshold_ = declare_parameter<int>("blockage_count_threshold");
3838
blockage_buffering_frames_ = declare_parameter<int>("blockage_buffering_frames");
3939
blockage_buffering_interval_ = declare_parameter<int>("blockage_buffering_interval");
40+
enable_dust_diag_ = declare_parameter<bool>("enable_dust_diag");
4041
dust_ratio_threshold_ = declare_parameter<float>("dust_ratio_threshold");
4142
dust_count_threshold_ = declare_parameter<int>("dust_count_threshold");
4243
dust_kernel_size_ = declare_parameter<int>("dust_kernel_size");
@@ -59,24 +60,27 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
5960
updater_.add(
6061
std::string(this->get_namespace()) + ": blockage_validation", this,
6162
&BlockageDiagComponent::onBlockageChecker);
62-
updater_.add(
63-
std::string(this->get_namespace()) + ": dust_validation", this,
64-
&BlockageDiagComponent::dustChecker);
63+
if (enable_dust_diag_) {
64+
updater_.add(
65+
std::string(this->get_namespace()) + ": dust_validation", this,
66+
&BlockageDiagComponent::dustChecker);
67+
}
6568
updater_.setPeriod(0.1);
66-
single_frame_dust_mask_pub =
67-
image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image");
68-
multi_frame_dust_mask_pub =
69-
image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image");
7069
lidar_depth_map_pub_ =
7170
image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map");
7271
blockage_mask_pub_ =
7372
image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image");
74-
blockage_dust_merged_pub =
75-
image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image");
7673
ground_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
7774
"blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS());
7875
sky_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
7976
"blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS());
77+
78+
single_frame_dust_mask_pub =
79+
image_transport::create_publisher(this, "blockage_diag/debug/single_frame_dust_mask_image");
80+
multi_frame_dust_mask_pub =
81+
image_transport::create_publisher(this, "blockage_diag/debug/multi_frame_dust_mask_image");
82+
blockage_dust_merged_pub =
83+
image_transport::create_publisher(this, "blockage_diag/debug/blockage_dust_merged_image");
8084
ground_dust_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
8185
"blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS());
8286
using std::placeholders::_1;
@@ -283,93 +287,103 @@ void BlockageDiagComponent::filter(
283287
sky_blockage_count_ = 0;
284288
}
285289
// dust
286-
cv::Mat ground_depth_map = lidar_depth_map_8u(
287-
cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_));
288-
cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
289-
cv::Mat ground_blank(
290-
vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
291-
cv::Mat single_dust_img(
292-
cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0));
293-
cv::Mat single_dust_ground_img = ground_depth_map.clone();
294-
cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img);
295-
cv::Mat dust_element = getStructuringElement(
296-
cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1),
297-
cv::Point(-1, -1));
298-
cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element);
299-
cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element);
300-
cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img);
301-
cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1);
302-
cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img);
303-
cv::Mat binarized_dust_mask_(
304-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
305-
cv::Mat multi_frame_dust_mask(
306-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
307-
cv::Mat multi_frame_ground_dust_result(
308-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
290+
if (enable_dust_diag_) {
291+
cv::Mat ground_depth_map = lidar_depth_map_8u(
292+
cv::Rect(0, horizontal_ring_id_, ideal_horizontal_bins, vertical_bins - horizontal_ring_id_));
293+
cv::Mat sky_blank(horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
294+
cv::Mat ground_blank(
295+
vertical_bins - horizontal_ring_id_, ideal_horizontal_bins, CV_8UC1, cv::Scalar(0));
296+
cv::Mat single_dust_img(
297+
cv::Size(lidar_depth_map_8u.rows, lidar_depth_map_8u.cols), CV_8UC1, cv::Scalar(0));
298+
cv::Mat single_dust_ground_img = ground_depth_map.clone();
299+
cv::inRange(single_dust_ground_img, 0, 1, single_dust_ground_img);
300+
cv::Mat dust_element = getStructuringElement(
301+
cv::MORPH_RECT, cv::Size(2 * dust_kernel_size_ + 1, 2 * dust_kernel_size_ + 1),
302+
cv::Point(-1, -1));
303+
cv::dilate(single_dust_ground_img, single_dust_ground_img, dust_element);
304+
cv::erode(single_dust_ground_img, single_dust_ground_img, dust_element);
305+
cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img);
306+
cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1);
307+
cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img);
308+
cv::Mat binarized_dust_mask_(
309+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
310+
cv::Mat multi_frame_dust_mask(
311+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
312+
cv::Mat multi_frame_ground_dust_result(
313+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
309314

310-
if (dust_buffering_interval_ == 0) {
311-
single_dust_img.copyTo(multi_frame_ground_dust_result);
312-
dust_buffering_frame_counter_ = 0;
313-
} else {
314-
binarized_dust_mask_ = single_dust_img / 255;
315-
if (dust_buffering_frame_counter_ >= dust_buffering_interval_) {
316-
dust_mask_buffer.push_back(binarized_dust_mask_);
315+
if (dust_buffering_interval_ == 0) {
316+
single_dust_img.copyTo(multi_frame_ground_dust_result);
317317
dust_buffering_frame_counter_ = 0;
318318
} else {
319-
dust_buffering_frame_counter_++;
320-
}
321-
for (const auto & binarized_dust_mask : dust_mask_buffer) {
322-
multi_frame_dust_mask += binarized_dust_mask;
319+
binarized_dust_mask_ = single_dust_img / 255;
320+
if (dust_buffering_frame_counter_ >= dust_buffering_interval_) {
321+
dust_mask_buffer.push_back(binarized_dust_mask_);
322+
dust_buffering_frame_counter_ = 0;
323+
} else {
324+
dust_buffering_frame_counter_++;
325+
}
326+
for (const auto & binarized_dust_mask : dust_mask_buffer) {
327+
multi_frame_dust_mask += binarized_dust_mask;
328+
}
329+
cv::inRange(
330+
multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(),
331+
multi_frame_ground_dust_result);
323332
}
324-
cv::inRange(
325-
multi_frame_dust_mask, dust_mask_buffer.size() - 1, dust_mask_buffer.size(),
326-
multi_frame_ground_dust_result);
327-
}
328-
cv::Mat single_frame_ground_dust_colorized(
329-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
330-
cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET);
331-
cv::Mat multi_frame_ground_dust_colorized;
332-
cv::Mat blockage_dust_merged_img(
333-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
334-
cv::Mat blockage_dust_merged_mask(
335-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
336-
blockage_dust_merged_img.setTo(
337-
cv::Vec3b(0, 0, 255), time_series_blockage_result); // red:blockage
338-
blockage_dust_merged_img.setTo(
339-
cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result); // yellow:dust
340-
sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg =
341-
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized)
342-
.toImageMsg();
343-
single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg);
344-
sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg =
345-
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized)
346-
.toImageMsg();
347-
multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg);
348-
sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg =
349-
cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg();
350-
lidar_depth_map_msg->header = input->header;
351-
lidar_depth_map_pub_.publish(lidar_depth_map_msg);
352-
cv::Mat blockage_mask_colorized;
353-
cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET);
354-
sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
355-
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
356-
blockage_mask_msg->header = input->header;
357-
blockage_mask_pub_.publish(blockage_mask_msg);
358-
cv::Mat blockage_dust_merged_colorized(
359-
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
360-
blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized);
361-
sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg =
362-
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized)
363-
.toImageMsg();
364-
if (ground_dust_ratio_ > dust_ratio_threshold_) {
365-
if (dust_frame_count_ < 2 * dust_count_threshold_) {
366-
dust_frame_count_++;
333+
cv::Mat single_frame_ground_dust_colorized(
334+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
335+
cv::applyColorMap(single_dust_img, single_frame_ground_dust_colorized, cv::COLORMAP_JET);
336+
cv::Mat multi_frame_ground_dust_colorized;
337+
cv::Mat blockage_dust_merged_img(
338+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
339+
cv::Mat blockage_dust_merged_mask(
340+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0));
341+
blockage_dust_merged_img.setTo(
342+
cv::Vec3b(0, 0, 255), time_series_blockage_result); // red:blockage
343+
blockage_dust_merged_img.setTo(
344+
cv::Vec3b(0, 255, 255), multi_frame_ground_dust_result); // yellow:dust
345+
sensor_msgs::msg::Image::SharedPtr single_frame_dust_mask_msg =
346+
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", single_frame_ground_dust_colorized)
347+
.toImageMsg();
348+
single_frame_dust_mask_pub.publish(single_frame_dust_mask_msg);
349+
sensor_msgs::msg::Image::SharedPtr multi_frame_dust_mask_msg =
350+
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", multi_frame_ground_dust_colorized)
351+
.toImageMsg();
352+
multi_frame_dust_mask_pub.publish(multi_frame_dust_mask_msg);
353+
sensor_msgs::msg::Image::SharedPtr lidar_depth_map_msg =
354+
cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", full_size_depth_map).toImageMsg();
355+
lidar_depth_map_msg->header = input->header;
356+
lidar_depth_map_pub_.publish(lidar_depth_map_msg);
357+
cv::Mat blockage_mask_colorized;
358+
cv::applyColorMap(time_series_blockage_result, blockage_mask_colorized, cv::COLORMAP_JET);
359+
sensor_msgs::msg::Image::SharedPtr blockage_mask_msg =
360+
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg();
361+
blockage_mask_msg->header = input->header;
362+
blockage_mask_pub_.publish(blockage_mask_msg);
363+
cv::Mat blockage_dust_merged_colorized(
364+
cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC3, cv::Scalar(0, 0, 0));
365+
blockage_dust_merged_img.copyTo(blockage_dust_merged_colorized);
366+
sensor_msgs::msg::Image::SharedPtr blockage_dust_merged_msg =
367+
cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_dust_merged_colorized)
368+
.toImageMsg();
369+
if (ground_dust_ratio_ > dust_ratio_threshold_) {
370+
if (dust_frame_count_ < 2 * dust_count_threshold_) {
371+
dust_frame_count_++;
372+
}
373+
} else {
374+
dust_frame_count_ = 0;
367375
}
368-
} else {
369-
dust_frame_count_ = 0;
376+
blockage_dust_merged_msg->header = input->header;
377+
blockage_dust_merged_pub.publish(blockage_dust_merged_msg);
378+
tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg;
379+
tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg;
380+
381+
ground_dust_ratio_ = static_cast<float>(cv::countNonZero(single_dust_ground_img)) /
382+
(single_dust_ground_img.cols * single_dust_ground_img.rows);
383+
ground_dust_ratio_msg.data = ground_dust_ratio_;
384+
ground_dust_ratio_msg.stamp = now();
385+
ground_dust_ratio_pub_->publish(ground_dust_ratio_msg);
370386
}
371-
blockage_dust_merged_msg->header = input->header;
372-
blockage_dust_merged_pub.publish(blockage_dust_merged_msg);
373387

374388
tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg;
375389
ground_blockage_ratio_msg.data = ground_blockage_ratio_;
@@ -380,14 +394,6 @@ void BlockageDiagComponent::filter(
380394
sky_blockage_ratio_msg.data = sky_blockage_ratio_;
381395
sky_blockage_ratio_msg.stamp = now();
382396
sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg);
383-
tier4_debug_msgs::msg::Float32Stamped blockage_ratio_msg;
384-
tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg;
385-
386-
ground_dust_ratio_ = static_cast<float>(cv::countNonZero(single_dust_ground_img)) /
387-
(single_dust_ground_img.cols * single_dust_ground_img.rows);
388-
ground_dust_ratio_msg.data = ground_dust_ratio_;
389-
ground_dust_ratio_msg.stamp = now();
390-
ground_dust_ratio_pub_->publish(ground_dust_ratio_msg);
391397
pcl::toROSMsg(*pcl_input, output);
392398
output.header = input->header;
393399
}

0 commit comments

Comments
 (0)