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";