From fa667dea4db84a1a30406f854edb01c188b349c5 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 20 Dec 2023 23:50:57 +0900 Subject: [PATCH 01/12] chore(blockage_diag): refactor Signed-off-by: badai-nguyen --- .../blockage_diag/blockage_diag_nodelet.hpp | 4 ++- .../launch/blockage_diag.launch.xml | 9 ++++--- .../blockage_diag/blockage_diag_nodelet.cpp | 25 +++++++------------ 3 files changed, 18 insertions(+), 20 deletions(-) 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..5da8f1806a220 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 @@ -80,7 +80,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 +89,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int dust_buffering_frame_counter_ = 0; int dust_count_threshold_; int dust_frame_count_ = 0; + double distance_coefficient_{327.67}; + double horizontal_resolution_{0.4}; 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..5cef28b95b1f3 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -5,7 +5,7 @@ - + @@ -15,7 +15,8 @@ - + + @@ -23,7 +24,7 @@ - + @@ -33,5 +34,7 @@ + + 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..1d044581aabf3 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -35,7 +35,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 +44,8 @@ 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"); + distance_coefficient_ = declare_parameter("distance_coefficient"); + horizontal_resolution_ = declare_parameter("horizontal_resolution"); } updater_.setHardwareID("blockage_diag"); @@ -150,15 +152,6 @@ 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; - } ideal_horizontal_bins = static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); @@ -195,12 +188,12 @@ void BlockageDiagComponent::filter( (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") { + if (is_channel_order_top2down_) { full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { + UINT16_MAX - distance_coefficient_ * p.distance; + } else { full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; + UINT16_MAX - distance_coefficient_ * p.distance; } } } @@ -403,8 +396,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( From 700eb08a39e49d9cb88d20e4af7aa2d6ca6ffa37 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 22 Dec 2023 12:36:08 +0900 Subject: [PATCH 02/12] chore: add error logger Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 1d044581aabf3..dff1c50ea0fe2 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -47,7 +47,12 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options distance_coefficient_ = declare_parameter("distance_coefficient"); horizontal_resolution_ = declare_parameter("horizontal_resolution"); } - + 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"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, From 62806bc9323469c606308aa13477d533059e5386 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 22 Dec 2023 13:02:24 +0900 Subject: [PATCH 03/12] chore: refactor Signed-off-by: badai-nguyen --- .../pointcloud_preprocessor/CMakeLists.txt | 1 + .../blockage_diagnostics_param_file.yaml | 13 ++++++++++ .../launch/blockage_diag.launch.xml | 26 ++----------------- 3 files changed, 16 insertions(+), 24 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..e8409a50e9d5a --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,13 @@ +/**: + 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 + distance_coefficient: 327.67 + horizontal_resolution: 0.4 diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 5cef28b95b1f3..5a93cb950b827 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -1,40 +1,18 @@ - - - - - - - - - - - - + - - - - - - - - - - - - + From 5a86763bef678839778f166d49b04df183f7a823 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 22 Dec 2023 13:02:41 +0900 Subject: [PATCH 04/12] docs: update readme Signed-off-by: badai-nguyen --- sensing/pointcloud_preprocessor/docs/blockage_diag.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 32e5a5869109d..ef3176f03f757 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -58,7 +58,7 @@ 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 | From 100a4820f152ce0715bb39388bf234089b2bc190 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 26 Dec 2023 00:27:25 +0900 Subject: [PATCH 05/12] chore: refactor Signed-off-by: badai-nguyen --- .../blockage_diag/blockage_diag_nodelet.hpp | 2 +- .../blockage_diag/blockage_diag_nodelet.cpp | 33 +++++++------------ 2 files changed, 13 insertions(+), 22 deletions(-) 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 5da8f1806a220..a42959c32d080 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 @@ -89,7 +89,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int dust_buffering_frame_counter_ = 0; int dust_count_threshold_; int dust_frame_count_ = 0; - double distance_coefficient_{327.67}; + double max_distance_range_{200.0}; double horizontal_resolution_{0.4}; public: 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 dff1c50ea0fe2..b03bdfd5a9dca 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -44,7 +44,7 @@ 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"); - distance_coefficient_ = declare_parameter("distance_coefficient"); + max_distance_range_ = declare_parameter("max_distance_range"); horizontal_resolution_ = declare_parameter("horizontal_resolution"); } if (vertical_bins_ <= horizontal_ring_id_) { @@ -161,11 +161,8 @@ void BlockageDiagComponent::filter( static_cast((angle_range_deg_[1] - 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()) { @@ -182,24 +179,18 @@ 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 (is_channel_order_top2down_) { - full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient_ * p.distance; - } else { - 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])) { + int horizontal_bin_index = + static_cast((azimuth_deg - angle_range_deg_[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; } } } From 001528a5bb0b2dc87e4190e96e00142d1276017e Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 26 Dec 2023 18:52:04 +0900 Subject: [PATCH 06/12] fix: break interval angle as continuous one Signed-off-by: badai-nguyen --- .../blockage_diag/blockage_diag_nodelet.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) 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 b03bdfd5a9dca..3725b9897580c 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -157,8 +157,12 @@ void BlockageDiagComponent::filter( std::scoped_lock lock(mutex_); int vertical_bins = vertical_bins_; int ideal_horizontal_bins; - ideal_horizontal_bins = - static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); + double compensate_angle = 0.0; + if (angle_range_deg_[0] > angle_range_deg_[1]) { + compensate_angle = 360.0; + } + 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); cv::Mat full_size_depth_map( @@ -181,9 +185,14 @@ void BlockageDiagComponent::filter( } else { for (const auto p : pcl_input->points) { double azimuth_deg = p.azimuth / 100.; - if ((azimuth_deg > angle_range_deg_[0]) && (azimuth_deg <= angle_range_deg_[1])) { - int horizontal_bin_index = - static_cast((azimuth_deg - angle_range_deg_[0]) / horizontal_resolution_); + 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_) { From e80a07c9fec24e0a137fb6d45bdcc1913603e883 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 26 Dec 2023 19:42:42 +0900 Subject: [PATCH 07/12] chore: update param file Signed-off-by: badai-nguyen --- .../config/blockage_diagnostics_param_file.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml index e8409a50e9d5a..e7747a40d88c1 100644 --- a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -9,5 +9,5 @@ dust_kernel_size: 2 dust_buffering_frames: 10 dust_buffering_interval: 1 - distance_coefficient: 327.67 + max_distance_range: 200.0 horizontal_resolution: 0.4 From fea22ccb1e412d96cc526c36aebca661403921a0 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 28 Dec 2023 15:34:51 +0900 Subject: [PATCH 08/12] docs: update readme Signed-off-by: badai-nguyen --- sensing/pointcloud_preprocessor/docs/blockage_diag.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index ef3176f03f757..99be26526e921 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -66,6 +66,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `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 From 6f44db2357162107c575ad0441c86013d9f0e9e2 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 28 Dec 2023 15:49:31 +0900 Subject: [PATCH 09/12] chore: typo Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 1 + 1 file changed, 1 insertion(+) 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 3725b9897580c..ed1199b597c29 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -158,6 +158,7 @@ void BlockageDiagComponent::filter( int vertical_bins = vertical_bins_; int ideal_horizontal_bins; 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; } From eb34b95827f1af4423aa019169feede7745bdc8e Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 9 Jan 2024 12:27:24 +0900 Subject: [PATCH 10/12] fix: multi-blockage-diag in 1 container died bug fix Signed-off-by: badai-nguyen --- .../blockage_diag/blockage_diag_nodelet.hpp | 4 ++++ .../src/blockage_diag/blockage_diag_nodelet.cpp | 6 ++---- 2 files changed, 6 insertions(+), 4 deletions(-) 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 a42959c32d080..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 @@ -91,6 +93,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter 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/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index ed1199b597c29..674fe3b195d14 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 @@ -47,6 +45,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options max_distance_range_ = declare_parameter("max_distance_range"); horizontal_resolution_ = declare_parameter("horizontal_resolution"); } + 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(), @@ -214,7 +214,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( @@ -292,7 +291,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( From 4fc3731dc8e9d25734b49f3238d6a637f3036628 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 19 Feb 2024 23:30:46 +0900 Subject: [PATCH 11/12] chore: update param_file Signed-off-by: badai-nguyen --- .../config/blockage_diagnostics_param_file.yaml | 1 + sensing/pointcloud_preprocessor/docs/blockage_diag.md | 1 + .../src/blockage_diag/blockage_diag_nodelet.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml index e7747a40d88c1..c932e7f62d5bc 100644 --- a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -11,3 +11,4 @@ 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 99be26526e921..082ae180fa3c9 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -63,6 +63,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `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 | 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 674fe3b195d14..f5d340921a2c4 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -44,6 +44,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options 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_); From 44f932423496e3d658f9fe0ef7df6c60d5321b59 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 27 Feb 2024 00:32:39 +0900 Subject: [PATCH 12/12] fix: Nan sky_blockage_ratio Signed-off-by: badai-nguyen --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) 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 f5d340921a2c4..4fc138725c8bd 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -54,6 +54,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options "The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"); return; } + updater_.setHardwareID("blockage_diag"); updater_.add( std::string(this->get_namespace()) + ": blockage_validation", this, @@ -249,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);