From 83bad1f9e30b208376022ff8a666840ec630dffa Mon Sep 17 00:00:00 2001 From: Yusuke Mizoguchi <33048729+swiftfile@users.noreply.github.com> Date: Wed, 31 Aug 2022 15:14:33 +0900 Subject: [PATCH] feat(blockage_diag): time series blockage diag (#1004) * Add basic pixel value sum function Signed-off-by: swift_file * update change sum topic(depth_map->mask_image) Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * update to use circular buffer Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * refactor : rename topic_name and publisher_name Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * fix : properly colorized Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * fix normalize process Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * fix bug Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * fix typo Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * update set initial parameter Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * update: change the number of frames to variable Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * feat :Periodically add frames to the buffer ,style Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * chore rename variable Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * add Diagnostics item Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * add debug statement of rosparam Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * update : time series processing integrated to blockage diag Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * chore Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * refactor: change variable name Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * delete non used publisher Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * docs: add description about new parameter Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * update flowchart Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file * fix: delete unnecessary item of Diagnotics Signed-off-by: swift_file * update:calc blockage_ratio by multi frame blockage mask Signed-off-by: swift_file * ci(pre-commit): autofix Signed-off-by: swift_file Signed-off-by: YusukeMizoguchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../docs/blockage_diag.md | 2 + .../image/blockage_diag_flowchart.drawio.svg | 2 +- .../blockage_diag/blockage_diag_nodelet.hpp | 2 + .../blockage_diag/blockage_diag_nodelet.cpp | 45 ++++++++++++++++--- 4 files changed, 45 insertions(+), 6 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index bd7a05ee0f9bd..6c51755768320 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -45,6 +45,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `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 | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg index cfa45e9761d28..b560e699dc273 100644 --- a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg +++ b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg @@ -1,4 +1,4 @@ -
Depth map
Depth map
no return region detection
no return region det...
small segment filter
small segment filter
ground region blockage extract
ground region blocka...input point clouddiagnosticsblockage maskText is not SVG - cannot display \ No newline at end of file +
input pointcloud
input pointcloud
Depth map
Depth map
no retrurn region 
detection
no retrurn region...
small segment
fillter
small segment...
buffering
buffering
extract no return region in (multi) frames
extract no return regi...
sky-range and
ground range
comparison
sky-range and...
blockage mask
blockage mask
diagnotics
diagnoticsText is not SVG - cannot display \ No newline at end of file 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 6e7a9b3454e74..54fb4c2fa2307 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 @@ -69,6 +69,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter uint sky_blockage_count_ = 0; uint blockage_count_threshold_; std::string lidar_model_; + uint buffering_frames_ = 100; + uint buffering_interval_ = 5; public: PCL_MAKE_ALIGNED_OPERATOR_NEW 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 3f9f427a8f93d..0f740eee6be40 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -16,7 +16,7 @@ #include "autoware_point_types/types.hpp" -#include +#include #include @@ -38,6 +38,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options 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)); } updater_.setHardwareID("blockage_diag"); @@ -73,7 +75,6 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) stat.add( "sky_blockage_range_deg", "[" + std::to_string(sky_blockage_range_deg_[0]) + "," + std::to_string(sky_blockage_range_deg_[1]) + "]"); - // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; @@ -151,16 +152,44 @@ void BlockageDiagComponent::filter( 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)); + + 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); + } else { + no_return_mask.copyTo(no_return_mask_result); + } cv::Mat ground_no_return_mask; cv::Mat sky_no_return_mask; - no_return_mask(cv::Rect(0, 0, horizontal_bins, horizontal_ring_id_)).copyTo(sky_no_return_mask); - 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_ = @@ -203,7 +232,7 @@ void BlockageDiagComponent::filter( lidar_depth_map_pub_.publish(lidar_depth_msg); cv::Mat blockage_mask_colorized; - cv::applyColorMap(no_return_mask, blockage_mask_colorized, cv::COLORMAP_JET); + 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; @@ -249,6 +278,12 @@ 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, "buffering_interval", buffering_interval_)) { + RCLCPP_DEBUG(get_logger(), "Setting new buffering_interval to: %d.", buffering_interval_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success";