From 9f701969245aca9775d61ce3a39b699f7ce3102d Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 27 Feb 2024 21:54:37 +0900 Subject: [PATCH] refactor(pointcloud_preprocessor): blockage_diag (#5932) * chore(blockage_diag): refactor Signed-off-by: badai-nguyen * chore: add error logger Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * fix: break interval angle as continuous one Signed-off-by: badai-nguyen * chore: update param file Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: multi-blockage-diag in 1 container died bug fix Signed-off-by: badai-nguyen * chore: update param_file Signed-off-by: badai-nguyen * fix: Nan sky_blockage_ratio Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../pointcloud_preprocessor/CMakeLists.txt | 1 + .../blockage_diagnostics_param_file.yaml | 14 ++++ .../docs/blockage_diag.md | 5 +- .../blockage_diag/blockage_diag_nodelet.hpp | 8 +- .../launch/blockage_diag.launch.xml | 27 +----- .../blockage_diag/blockage_diag_nodelet.cpp | 82 ++++++++++--------- 6 files changed, 73 insertions(+), 64 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index c5f15ec4e49d5..219f43e48730a 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -211,6 +211,7 @@ ament_export_targets(export_${PROJECT_NAME}) ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 0000000000000..c932e7f62d5bc --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + max_distance_range: 200.0 + horizontal_resolution: 0.4 + blockage_kernel: 10 diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 32e5a5869109d..082ae180fa3c9 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -58,14 +58,17 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | | `angle_range` | vector | The effective range of LiDAR | | `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | +| `is_channel_order_top2down` | bool | If the lidar channels are indexed from top to down | | `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | | `blockage_buffering_interval` | int | The interval of buffering about blockage detection | | `dust_ratio_threshold` | float | The threshold of dusty area ratio | | `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `blockage_kernel` | int | The kernel size of morphology processing the detected blockage area | | `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | | `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | | `dust_buffering_interval` | int | The interval of buffering about dusty area detection | +| `max_distance_range` | double | Maximum view range for the LiDAR | +| `horizontal_resolution` | double | The horizontal resolution of depth map image [deg/pixel] | ## Assumptions / Known limits 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 66e14c418b52c..c1cf1e59f0343 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 @@ -33,6 +33,8 @@ #include #endif +#include + #include #include @@ -80,7 +82,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int ground_blockage_count_ = 0; int sky_blockage_count_ = 0; int blockage_count_threshold_; - std::string lidar_model_; + bool is_channel_order_top2down_; int blockage_buffering_frames_; int blockage_buffering_interval_; int dust_kernel_size_; @@ -89,6 +91,10 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int dust_buffering_frame_counter_ = 0; int dust_count_threshold_; int dust_frame_count_ = 0; + double max_distance_range_{200.0}; + double horizontal_resolution_{0.4}; + boost::circular_buffer no_return_mask_buffer{1}; + boost::circular_buffer dust_mask_buffer{1}; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 8fa5796e2802d..5a93cb950b827 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -1,37 +1,18 @@ - - - - - - - - - - - - + + - - - - - - - - - - - + + 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 06da91e61b3bd..4fc138725c8bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -16,8 +16,6 @@ #include "autoware_point_types/types.hpp" -#include - #include #include @@ -35,7 +33,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); vertical_bins_ = declare_parameter("vertical_bins"); angle_range_deg_ = declare_parameter>("angle_range"); - lidar_model_ = declare_parameter("model"); + is_channel_order_top2down_ = declare_parameter("is_channel_order_top2down"); blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); @@ -44,6 +42,17 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options dust_kernel_size_ = declare_parameter("dust_kernel_size"); dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); + max_distance_range_ = declare_parameter("max_distance_range"); + horizontal_resolution_ = declare_parameter("horizontal_resolution"); + blockage_kernel_ = declare_parameter("blockage_kernel"); + } + dust_mask_buffer.set_capacity(dust_buffering_frames_); + no_return_mask_buffer.set_capacity(blockage_buffering_frames_); + if (vertical_bins_ <= horizontal_ring_id_) { + RCLCPP_ERROR( + this->get_logger(), + "The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"); + return; } updater_.setHardwareID("blockage_diag"); @@ -150,24 +159,17 @@ void BlockageDiagComponent::filter( std::scoped_lock lock(mutex_); int vertical_bins = vertical_bins_; int ideal_horizontal_bins; - float distance_coefficient = 327.67f; - float horizontal_resolution_ = 0.4f; - if (lidar_model_ == "Pandar40P") { - distance_coefficient = 327.67f; - horizontal_resolution_ = 0.4f; - } else if (lidar_model_ == "PandarQT") { - distance_coefficient = 3276.75f; - horizontal_resolution_ = 0.6f; + double compensate_angle = 0.0; + // Check the case when angle_range_deg_[1] exceed 360 and shifted the range to 0~360 + if (angle_range_deg_[0] > angle_range_deg_[1]) { + compensate_angle = 360.0; } - ideal_horizontal_bins = - static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); + ideal_horizontal_bins = static_cast( + (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - std::vector horizontal_bin_reference(ideal_horizontal_bins); - std::vector> each_ring_pointcloud(vertical_bins); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); cv::Mat lidar_depth_map_8u( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { @@ -184,24 +186,23 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (int i = 0; i < ideal_horizontal_bins; ++i) { - horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; - } for (const auto p : pcl_input->points) { - for (int horizontal_bin = 0; - horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { - if ( - (p.azimuth / 100 > - (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && - (p.azimuth / 100 <= - (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { - if (lidar_model_ == "Pandar40P") { - full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } + double azimuth_deg = p.azimuth / 100.; + if ( + ((azimuth_deg > angle_range_deg_[0]) && + (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || + ((azimuth_deg + compensate_angle > angle_range_deg_[0]) && + (azimuth_deg < angle_range_deg_[1]))) { + double current_angle_range = (azimuth_deg + compensate_angle - angle_range_deg_[0]); + int horizontal_bin_index = static_cast(current_angle_range / horizontal_resolution_) % + static_cast(360.0 / horizontal_resolution_); + uint16_t depth_intensity = + UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); + if (is_channel_order_top2down_) { + full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + } else { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + depth_intensity; } } } @@ -215,7 +216,6 @@ void BlockageDiagComponent::filter( cv::Point(blockage_kernel_, blockage_kernel_)); cv::erode(no_return_mask, erosion_dst, blockage_element); cv::dilate(erosion_dst, no_return_mask, blockage_element); - static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); cv::Mat time_series_blockage_result( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat time_series_blockage_mask( @@ -250,8 +250,13 @@ void BlockageDiagComponent::filter( ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(ideal_horizontal_bins * horizontal_ring_id_); + + if (horizontal_ring_id_ == 0) { + sky_blockage_ratio_ = 0.0f; + } else { + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); + } if (ground_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); @@ -293,7 +298,6 @@ void BlockageDiagComponent::filter( 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); - static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); cv::Mat binarized_dust_mask_( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat multi_frame_dust_mask( @@ -403,8 +407,8 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( RCLCPP_DEBUG( get_logger(), "Setting new blockage_count_threshold to: %d.", blockage_count_threshold_); } - if (get_param(p, "model", lidar_model_)) { - RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %s. ", lidar_model_.c_str()); + if (get_param(p, "is_channel_order_top2down", is_channel_order_top2down_)) { + RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %d. ", is_channel_order_top2down_); } if (get_param(p, "angle_range", angle_range_deg_)) { RCLCPP_DEBUG(