@@ -37,6 +37,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
37
37
blockage_count_threshold_ = declare_parameter<int >(" blockage_count_threshold" );
38
38
blockage_buffering_frames_ = declare_parameter<int >(" blockage_buffering_frames" );
39
39
blockage_buffering_interval_ = declare_parameter<int >(" blockage_buffering_interval" );
40
+ enable_dust_diag_ = declare_parameter<bool >(" enable_dust_diag" );
40
41
dust_ratio_threshold_ = declare_parameter<float >(" dust_ratio_threshold" );
41
42
dust_count_threshold_ = declare_parameter<int >(" dust_count_threshold" );
42
43
dust_kernel_size_ = declare_parameter<int >(" dust_kernel_size" );
@@ -59,24 +60,27 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
59
60
updater_.add (
60
61
std::string (this ->get_namespace ()) + " : blockage_validation" , this ,
61
62
&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
+ }
65
68
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" );
70
69
lidar_depth_map_pub_ =
71
70
image_transport::create_publisher (this , " blockage_diag/debug/lidar_depth_map" );
72
71
blockage_mask_pub_ =
73
72
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" );
76
73
ground_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
77
74
" blockage_diag/debug/ground_blockage_ratio" , rclcpp::SensorDataQoS ());
78
75
sky_blockage_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
79
76
" 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" );
80
84
ground_dust_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
81
85
" blockage_diag/debug/ground_dust_ratio" , rclcpp::SensorDataQoS ());
82
86
using std::placeholders::_1;
@@ -283,93 +287,103 @@ void BlockageDiagComponent::filter(
283
287
sky_blockage_count_ = 0 ;
284
288
}
285
289
// 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 ));
309
314
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);
317
317
dust_buffering_frame_counter_ = 0 ;
318
318
} 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);
323
332
}
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 ;
367
375
}
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);
370
386
}
371
- blockage_dust_merged_msg->header = input->header ;
372
- blockage_dust_merged_pub.publish (blockage_dust_merged_msg);
373
387
374
388
tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg;
375
389
ground_blockage_ratio_msg.data = ground_blockage_ratio_;
@@ -380,14 +394,6 @@ void BlockageDiagComponent::filter(
380
394
sky_blockage_ratio_msg.data = sky_blockage_ratio_;
381
395
sky_blockage_ratio_msg.stamp = now ();
382
396
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);
391
397
pcl::toROSMsg (*pcl_input, output);
392
398
output.header = input->header ;
393
399
}
0 commit comments