From 6711e656e129d953c3ea3ac4d57fd77da5fced8b Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Wed, 22 Jun 2022 18:39:00 +0900 Subject: [PATCH 01/23] Add basic pixel value sum function Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 1 + .../src/blockage_diag/blockage_diag_nodelet.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+) 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..0dd8cb13ce60b 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 @@ -50,6 +50,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; + image_transport::Publisher debug_image_pub_; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; 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..335245fcc33c7 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -50,6 +50,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); + debug_image_pub_ = image_transport::create_publisher(this,"blockage_diag/debug/debug_image"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); @@ -151,6 +152,12 @@ 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); + static std::vector depth_map_buffer; + depth_map_buffer.emplace_back(lidar_depth_map); + static cv::Mat depth_map_diff_sum(cv::Size(horizontal_bins,vertical_bins),CV_8UC1,cv::Scalar(0)); + for (auto ite = depth_map_buffer.begin();ite!=depth_map_buffer.end();++ite){ + depth_map_diff_sum += *ite; + } cv::Mat erosion_dst; cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT, cv::Size(2 * erode_kernel_ + 1, 2 * erode_kernel_ + 1), @@ -208,6 +215,13 @@ void BlockageDiagComponent::filter( cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); blockage_mask_msg->header = input->header; blockage_mask_pub_.publish(blockage_mask_msg); + cv::Mat depth_map_sum_colorized; + cv::applyColorMap(depth_map_diff_sum, depth_map_sum_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr depth_map_sum_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", depth_map_sum_colorized).toImageMsg(); + depth_map_sum_msg->header = input->header; + debug_image_pub_.publish(depth_map_sum_msg); + } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; From 02811d44f1cb2d1459d4e0f3e8f8ad4abc16250c Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Fri, 22 Apr 2022 13:13:49 +0900 Subject: [PATCH 02/23] update change sum topic(depth_map->mask_image) Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 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 335245fcc33c7..2a7a8c3e2878f 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -152,12 +152,9 @@ 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); - static std::vector depth_map_buffer; - depth_map_buffer.emplace_back(lidar_depth_map); - static cv::Mat depth_map_diff_sum(cv::Size(horizontal_bins,vertical_bins),CV_8UC1,cv::Scalar(0)); - for (auto ite = depth_map_buffer.begin();ite!=depth_map_buffer.end();++ite){ - depth_map_diff_sum += *ite; - } + static std::vector no_return_mask_buffer; + no_return_mask_buffer.emplace_back(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), @@ -170,6 +167,10 @@ void BlockageDiagComponent::filter( no_return_mask( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); + static cv::Mat no_return_mask_sum(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + for (const auto &frame:no_return_mask_buffer){ + no_return_mask_sum += frame; + } ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); @@ -215,12 +216,13 @@ void BlockageDiagComponent::filter( cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); blockage_mask_msg->header = input->header; blockage_mask_pub_.publish(blockage_mask_msg); - cv::Mat depth_map_sum_colorized; - cv::applyColorMap(depth_map_diff_sum, depth_map_sum_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr depth_map_sum_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", depth_map_sum_colorized).toImageMsg(); - depth_map_sum_msg->header = input->header; - debug_image_pub_.publish(depth_map_sum_msg); + + cv::Mat time_series_mask_result_colorized; + cv::applyColorMap(no_return_mask_sum, time_series_mask_result_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr mask_result_sum_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_mask_result_colorized).toImageMsg(); + mask_result_sum_msg->header = input->header; + debug_image_pub_.publish(mask_result_sum_msg); } From c8f52ee7a63c494e513b66ee9e0c476d0c9c8e58 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Fri, 22 Apr 2022 15:39:34 +0900 Subject: [PATCH 03/23] update to use circular buffer Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 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 2a7a8c3e2878f..17745c9d0d375 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -17,6 +17,7 @@ #include "autoware_point_types/types.hpp" #include +#include #include @@ -152,8 +153,6 @@ 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); - static std::vector no_return_mask_buffer; - no_return_mask_buffer.emplace_back(no_return_mask); cv::Mat erosion_dst; cv::Mat element = cv::getStructuringElement( @@ -167,10 +166,14 @@ void BlockageDiagComponent::filter( no_return_mask( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); - static cv::Mat no_return_mask_sum(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - for (const auto &frame:no_return_mask_buffer){ - no_return_mask_sum += frame; + cv::Mat time_series_blockage_mask(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + // TODO(yusuke-mizoguchi): to adjust the number of frames" [todo] + static boost::circular_buffer no_return_mask_buffer(10); + no_return_mask_buffer.push_back(no_return_mask); + for (const auto& mask:no_return_mask_buffer) { + time_series_blockage_mask +=mask; } + time_series_blockage_mask = time_series_blockage_mask*255/no_return_mask_buffer.size(); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); @@ -217,13 +220,12 @@ void BlockageDiagComponent::filter( blockage_mask_msg->header = input->header; blockage_mask_pub_.publish(blockage_mask_msg); - cv::Mat time_series_mask_result_colorized; - cv::applyColorMap(no_return_mask_sum, time_series_mask_result_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr mask_result_sum_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_mask_result_colorized).toImageMsg(); - mask_result_sum_msg->header = input->header; - debug_image_pub_.publish(mask_result_sum_msg); - + cv::Mat time_series_blockage_mask_colorized; + cv::applyColorMap(time_series_blockage_mask, time_series_blockage_mask_colorized, cv::COLORMAP_JET); + sensor_msgs::msg::Image::SharedPtr time_series_blockage_mask_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_blockage_mask_colorized).toImageMsg(); + time_series_blockage_mask_msg->header = input->header; + debug_image_pub_.publish(time_series_blockage_mask_msg); } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; From 3143aafefa4d1e09ad55966a504efdb33d75a12b Mon Sep 17 00:00:00 2001 From: swift_file Date: Tue, 26 Apr 2022 14:55:55 +0900 Subject: [PATCH 04/23] refactor : rename topic_name and publisher_name Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 2 +- .../src/blockage_diag/blockage_diag_nodelet.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 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 0dd8cb13ce60b..6e459c92e6c06 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 @@ -50,7 +50,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; - image_transport::Publisher debug_image_pub_; + image_transport::Publisher time_series_blockage_mask_pub_; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; 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 17745c9d0d375..3dd3558f371e0 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -51,7 +51,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - debug_image_pub_ = image_transport::create_publisher(this,"blockage_diag/debug/debug_image"); + time_series_blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); @@ -225,7 +225,7 @@ void BlockageDiagComponent::filter( sensor_msgs::msg::Image::SharedPtr time_series_blockage_mask_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_blockage_mask_colorized).toImageMsg(); time_series_blockage_mask_msg->header = input->header; - debug_image_pub_.publish(time_series_blockage_mask_msg); + time_series_blockage_mask_pub_.publish(time_series_blockage_mask_msg); } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; From 9138069cfc0c94b8ef0d62f210a74c7694c73193 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 26 Apr 2022 15:02:42 +0900 Subject: [PATCH 05/23] fix : properly colorized Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 6 ++++-- 1 file changed, 4 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 3dd3558f371e0..d276fbf2ad3f6 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -168,12 +168,14 @@ void BlockageDiagComponent::filter( .copyTo(ground_no_return_mask); cv::Mat time_series_blockage_mask(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // TODO(yusuke-mizoguchi): to adjust the number of frames" [todo] + cv::Mat no_return_mask_normalized(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); static boost::circular_buffer no_return_mask_buffer(10); - no_return_mask_buffer.push_back(no_return_mask); + no_return_mask_normalized= no_return_mask / no_return_mask_buffer.size(); + no_return_mask_buffer.push_back(no_return_mask_normalized); for (const auto& mask:no_return_mask_buffer) { time_series_blockage_mask +=mask; } - time_series_blockage_mask = time_series_blockage_mask*255/no_return_mask_buffer.size(); +// time_series_blockage_mask = time_series_blockage_mask*(255/no_return_mask_buffer.size()); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); From f4baecbca9dcf9e8ea3e752cb9143d19ae135778 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 10 May 2022 11:10:17 +0900 Subject: [PATCH 06/23] fix normalize process Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 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 d276fbf2ad3f6..07e01d1c036da 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -168,14 +168,16 @@ void BlockageDiagComponent::filter( .copyTo(ground_no_return_mask); cv::Mat time_series_blockage_mask(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // TODO(yusuke-mizoguchi): to adjust the number of frames" [todo] - cv::Mat no_return_mask_normalized(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::Mat no_return_mask_binalized(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); static boost::circular_buffer no_return_mask_buffer(10); - no_return_mask_normalized= no_return_mask / no_return_mask_buffer.size(); - no_return_mask_buffer.push_back(no_return_mask_normalized); - for (const auto& mask:no_return_mask_buffer) { + no_return_mask_binalized= no_return_mask / 255; + no_return_mask_buffer.push_back(no_return_mask_binalized); + for (const auto& binary_mask:no_return_mask_buffer) { time_series_blockage_mask +=mask; + time_series_blockage_mask += binary_mask; } // time_series_blockage_mask = time_series_blockage_mask*(255/no_return_mask_buffer.size()); + cv::inRange(time_series_blockage_mask,no_return_mask_buffer.size()-1,no_return_mask_buffer.size(),time_series_blockage_mask); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); From 4de41b7d98496a6353e17e9dfcf0cbdfdb233c24 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 10 May 2022 11:12:41 +0900 Subject: [PATCH 07/23] fix bug Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 1 - 1 file changed, 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 07e01d1c036da..30ca9d7bb1f93 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -173,7 +173,6 @@ void BlockageDiagComponent::filter( no_return_mask_binalized= no_return_mask / 255; no_return_mask_buffer.push_back(no_return_mask_binalized); for (const auto& binary_mask:no_return_mask_buffer) { - time_series_blockage_mask +=mask; time_series_blockage_mask += binary_mask; } // time_series_blockage_mask = time_series_blockage_mask*(255/no_return_mask_buffer.size()); From 5b13ab994878f51f1e78c54b3040c8f73b7f9027 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 10 May 2022 11:17:05 +0900 Subject: [PATCH 08/23] fix typo Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 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 30ca9d7bb1f93..ef99ab8682d5f 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -168,10 +168,10 @@ void BlockageDiagComponent::filter( .copyTo(ground_no_return_mask); cv::Mat time_series_blockage_mask(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); // TODO(yusuke-mizoguchi): to adjust the number of frames" [todo] - cv::Mat no_return_mask_binalized(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 boost::circular_buffer no_return_mask_buffer(10); - no_return_mask_binalized= no_return_mask / 255; - no_return_mask_buffer.push_back(no_return_mask_binalized); + no_return_mask_binarized= no_return_mask / 255; + no_return_mask_buffer.push_back(no_return_mask_binarized); for (const auto& binary_mask:no_return_mask_buffer) { time_series_blockage_mask += binary_mask; } From c52f470387a17a5ebcbbaac760159923592ea894 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 10 May 2022 15:48:14 +0900 Subject: [PATCH 09/23] update set initial parameter Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 1 + .../src/blockage_diag/blockage_diag_nodelet.cpp | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) 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 6e459c92e6c06..f667fdcf89ccb 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 @@ -70,6 +70,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter uint sky_blockage_count_ = 0; uint blockage_count_threshold_; std::string lidar_model_; + uint time_series_blockage_frames_ = 100; 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 ef99ab8682d5f..a828f76425698 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -39,6 +39,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)); + time_series_blockage_frames_ = + static_cast(declare_parameter("time_series_blockage_frames", 100)); } updater_.setHardwareID("blockage_diag"); @@ -75,7 +77,7 @@ 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]) + "]"); - + stat.add("time_series_blockage_frames", std::to_string(time_series_blockage_frames_)); // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; @@ -270,6 +272,11 @@ 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, "time_series_blockage_frames_", time_series_blockage_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new time_series_blockage_frames to: %d.", + time_series_blockage_frames_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; From 74c24fee49d06fdb357fc1e16289065f9ee72152 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 10 May 2022 15:55:16 +0900 Subject: [PATCH 10/23] update: change the number of frames to variable Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 3 +-- 1 file changed, 1 insertion(+), 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 a828f76425698..fa193892c32f4 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -169,9 +169,8 @@ void BlockageDiagComponent::filter( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); cv::Mat time_series_blockage_mask(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - // TODO(yusuke-mizoguchi): to adjust the number of frames" [todo] cv::Mat no_return_mask_binarized(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - static boost::circular_buffer no_return_mask_buffer(10); + static boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); no_return_mask_binarized= no_return_mask / 255; no_return_mask_buffer.push_back(no_return_mask_binarized); for (const auto& binary_mask:no_return_mask_buffer) { From f2691fe5e56062323ef041718101285e398f4a1a Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 31 May 2022 15:45:21 +0900 Subject: [PATCH 11/23] feat :Periodically add frames to the buffer ,style Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.cpp | 42 ++++++++++++------- 1 file changed, 28 insertions(+), 14 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 fa193892c32f4..806d121ba8d22 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,8 @@ #include "autoware_point_types/types.hpp" -#include #include +#include #include @@ -53,7 +53,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - time_series_blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); + time_series_blockage_mask_pub_ = + image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); @@ -168,16 +169,27 @@ void BlockageDiagComponent::filter( no_return_mask( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); - 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)); + 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 boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); - no_return_mask_binarized= no_return_mask / 255; - no_return_mask_buffer.push_back(no_return_mask_binarized); - for (const auto& binary_mask:no_return_mask_buffer) { - time_series_blockage_mask += binary_mask; - } -// time_series_blockage_mask = time_series_blockage_mask*(255/no_return_mask_buffer.size()); - cv::inRange(time_series_blockage_mask,no_return_mask_buffer.size()-1,no_return_mask_buffer.size(),time_series_blockage_mask); + no_return_mask_binarized = no_return_mask / 255; + static int frame_count; + int interval_frames = 5; + frame_count++; + if (frame_count == interval_frames) { + 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::Mat time_series_blockage_result( + cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + cv::inRange( + time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), + time_series_blockage_result); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); @@ -225,10 +237,12 @@ void BlockageDiagComponent::filter( blockage_mask_pub_.publish(blockage_mask_msg); cv::Mat time_series_blockage_mask_colorized; - cv::applyColorMap(time_series_blockage_mask, time_series_blockage_mask_colorized, cv::COLORMAP_JET); + cv::applyColorMap( + time_series_blockage_result, time_series_blockage_mask_colorized, cv::COLORMAP_JET); sensor_msgs::msg::Image::SharedPtr time_series_blockage_mask_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_blockage_mask_colorized).toImageMsg(); - time_series_blockage_mask_msg->header = input->header; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_blockage_mask_colorized) + .toImageMsg(); + time_series_blockage_mask_msg->header = input->header; time_series_blockage_mask_pub_.publish(time_series_blockage_mask_msg); } From cd707def3d1d0b95bec603f1a1626d365dd77425 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 24 May 2022 18:34:21 +0900 Subject: [PATCH 12/23] chore rename variable Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 1 + .../src/blockage_diag/blockage_diag_nodelet.cpp | 9 +++++---- 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 f667fdcf89ccb..cad797fe4be50 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 @@ -71,6 +71,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter uint blockage_count_threshold_; std::string lidar_model_; uint time_series_blockage_frames_ = 100; + uint time_series_blockage_interval_frames_ = 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 806d121ba8d22..dc9b479aa56e9 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -41,6 +41,8 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options static_cast(declare_parameter("blockage_count_threshold", 50)); time_series_blockage_frames_ = static_cast(declare_parameter("time_series_blockage_frames", 100)); + time_series_blockage_frames_ = + static_cast(declare_parameter("time_series_blockage_interval_frames", 5)); } updater_.setHardwareID("blockage_diag"); @@ -175,10 +177,9 @@ void BlockageDiagComponent::filter( cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); static boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); no_return_mask_binarized = no_return_mask / 255; - static int frame_count; - int interval_frames = 5; + static uint frame_count; frame_count++; - if (frame_count == interval_frames) { + if (frame_count == time_series_blockage_interval_frames_) { no_return_mask_buffer.push_back(no_return_mask_binarized); frame_count = 0; } @@ -285,7 +286,7 @@ 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, "time_series_blockage_frames_", time_series_blockage_frames_)) { + if (get_param(p, "time_series_blockage_frames", time_series_blockage_frames_)) { RCLCPP_DEBUG( get_logger(), "Setting new time_series_blockage_frames to: %d.", time_series_blockage_frames_); From d9cb24a15c02894ff813274db0494b1b359281db Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 24 May 2022 18:36:04 +0900 Subject: [PATCH 13/23] add Diagnostics item Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 dc9b479aa56e9..af62a830b2b49 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -81,6 +81,8 @@ void BlockageDiagComponent::onBlockageChecker(DiagnosticStatusWrapper & stat) "sky_blockage_range_deg", "[" + std::to_string(sky_blockage_range_deg_[0]) + "," + std::to_string(sky_blockage_range_deg_[1]) + "]"); stat.add("time_series_blockage_frames", std::to_string(time_series_blockage_frames_)); + stat.add( + "time_series_blockage_interval_frames", std::to_string(time_series_blockage_interval_frames_)); // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; From 6f269cf667078500a7a3d80fd0a3afacfa096e96 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 24 May 2022 18:37:48 +0900 Subject: [PATCH 14/23] add debug statement of rosparam Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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 af62a830b2b49..99968f18ce9cf 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -293,6 +293,11 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( get_logger(), "Setting new time_series_blockage_frames to: %d.", time_series_blockage_frames_); } + if (get_param(p, "time_series_blockage_interval_frames", time_series_blockage_interval_frames_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new time_series_blockage_interval_frames to: %d.", + time_series_blockage_frames_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; From 20077932938d6a21197dd3c25eb000d2e847ddb0 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 31 May 2022 15:50:32 +0900 Subject: [PATCH 15/23] update : time series processing integrated to blockage diag Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.cpp | 52 +++++++++---------- 1 file changed, 25 insertions(+), 27 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 99968f18ce9cf..26d7cdd0e9cd9 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,8 @@ #include "autoware_point_types/types.hpp" -#include #include +#include #include @@ -41,7 +41,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options static_cast(declare_parameter("blockage_count_threshold", 50)); time_series_blockage_frames_ = static_cast(declare_parameter("time_series_blockage_frames", 100)); - time_series_blockage_frames_ = + time_series_blockage_interval_frames_ = static_cast(declare_parameter("time_series_blockage_interval_frames", 5)); } @@ -55,8 +55,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - time_series_blockage_mask_pub_ = - image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); + time_series_blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); @@ -173,26 +172,33 @@ void BlockageDiagComponent::filter( no_return_mask( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); + + static boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); + + cv::Mat no_return_mask_result( + cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(255)); 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 boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); - no_return_mask_binarized = no_return_mask / 255; + static uint frame_count; frame_count++; - if (frame_count == time_series_blockage_interval_frames_) { - 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; + if (time_series_blockage_interval_frames_ != 0) { + no_return_mask_binarized = no_return_mask / 255; + if (frame_count == time_series_blockage_interval_frames_) { + 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 time_series_blockage_result( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); - cv::inRange( - time_series_blockage_mask, no_return_mask_buffer.size() - 1, no_return_mask_buffer.size(), - time_series_blockage_result); ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); @@ -233,20 +239,12 @@ 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; blockage_mask_pub_.publish(blockage_mask_msg); - cv::Mat time_series_blockage_mask_colorized; - cv::applyColorMap( - time_series_blockage_result, time_series_blockage_mask_colorized, cv::COLORMAP_JET); - sensor_msgs::msg::Image::SharedPtr time_series_blockage_mask_msg = - cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", time_series_blockage_mask_colorized) - .toImageMsg(); - time_series_blockage_mask_msg->header = input->header; - time_series_blockage_mask_pub_.publish(time_series_blockage_mask_msg); } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; @@ -296,7 +294,7 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( if (get_param(p, "time_series_blockage_interval_frames", time_series_blockage_interval_frames_)) { RCLCPP_DEBUG( get_logger(), "Setting new time_series_blockage_interval_frames to: %d.", - time_series_blockage_frames_); + time_series_blockage_interval_frames_); } rcl_interfaces::msg::SetParametersResult result; result.successful = true; From 90104730c3dd247caf42bcc26aba713c2a267923 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Thu, 26 May 2022 17:33:54 +0900 Subject: [PATCH 16/23] chore Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 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 26d7cdd0e9cd9..91811e458886c 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -176,7 +176,7 @@ void BlockageDiagComponent::filter( static boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); cv::Mat no_return_mask_result( - cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(255)); + 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( From 7722e7655dd6954dd08e9834cef25d4e1e471343 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 31 May 2022 13:31:48 +0900 Subject: [PATCH 17/23] refactor: change variable name Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 4 +-- .../blockage_diag/blockage_diag_nodelet.cpp | 32 +++++++------------ 2 files changed, 14 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 cad797fe4be50..9b27287375629 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 @@ -70,8 +70,8 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter uint sky_blockage_count_ = 0; uint blockage_count_threshold_; std::string lidar_model_; - uint time_series_blockage_frames_ = 100; - uint time_series_blockage_interval_frames_ = 5; + 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 91811e458886c..77f22aa3e833c 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -39,10 +39,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)); - time_series_blockage_frames_ = - static_cast(declare_parameter("time_series_blockage_frames", 100)); - time_series_blockage_interval_frames_ = - static_cast(declare_parameter("time_series_blockage_interval_frames", 5)); + buffering_frames_ = static_cast(declare_parameter("buffering_frames", 100)); + buffering_interval_ = static_cast(declare_parameter("buffering_interval", 5)); } updater_.setHardwareID("blockage_diag"); @@ -79,9 +77,8 @@ 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]) + "]"); - stat.add("time_series_blockage_frames", std::to_string(time_series_blockage_frames_)); - stat.add( - "time_series_blockage_interval_frames", std::to_string(time_series_blockage_interval_frames_)); + stat.add("buffering_frames", std::to_string(buffering_frames_)); + stat.add("buffering_interval", std::to_string(buffering_interval_)); // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; @@ -173,10 +170,9 @@ void BlockageDiagComponent::filter( cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) .copyTo(ground_no_return_mask); - static boost::circular_buffer no_return_mask_buffer(time_series_blockage_frames_); + 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 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( @@ -184,9 +180,9 @@ void BlockageDiagComponent::filter( static uint frame_count; frame_count++; - if (time_series_blockage_interval_frames_ != 0) { + if (buffering_interval_ != 0) { no_return_mask_binarized = no_return_mask / 255; - if (frame_count == time_series_blockage_interval_frames_) { + if (frame_count == buffering_interval_) { no_return_mask_buffer.push_back(no_return_mask_binarized); frame_count = 0; } @@ -286,15 +282,11 @@ 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, "time_series_blockage_frames", time_series_blockage_frames_)) { - RCLCPP_DEBUG( - get_logger(), "Setting new time_series_blockage_frames to: %d.", - time_series_blockage_frames_); + if (get_param(p, "buffering_frames", buffering_frames_)) { + RCLCPP_DEBUG(get_logger(), "Setting new buffering_frames to: %d.", buffering_frames_); } - if (get_param(p, "time_series_blockage_interval_frames", time_series_blockage_interval_frames_)) { - RCLCPP_DEBUG( - get_logger(), "Setting new time_series_blockage_interval_frames to: %d.", - time_series_blockage_interval_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; From b303c3455a5ff0d9dd3a6e672d61ca607863b936 Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 31 May 2022 16:01:09 +0900 Subject: [PATCH 18/23] delete non used publisher Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../blockage_diag/blockage_diag_nodelet.hpp | 1 - .../src/blockage_diag/blockage_diag_nodelet.cpp | 2 -- 2 files changed, 3 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 9b27287375629..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 @@ -50,7 +50,6 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher lidar_depth_map_pub_; image_transport::Publisher blockage_mask_pub_; - image_transport::Publisher time_series_blockage_mask_pub_; rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; 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 77f22aa3e833c..9a470a63d93be 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,6 @@ #include "autoware_point_types/types.hpp" -#include #include #include @@ -53,7 +52,6 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options image_transport::create_publisher(this, "blockage_diag/debug/lidar_depth_map"); blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); - time_series_blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/time_series_blockage_mask"); ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); From c505f64588f5c3a173eda410b03cb5b49f95110d Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Tue, 31 May 2022 13:57:10 +0900 Subject: [PATCH 19/23] docs: add description about new parameter Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- sensing/pointcloud_preprocessor/docs/blockage_diag.md | 2 ++ .../docs/image/blockage_diag_flowchart.drawio.svg | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index bd7a05ee0f9bd..9b6c0456dfdf7 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..3a41ffd4d4b4b 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
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 re...
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 From 35480133dbf356a62f35f1d6f95fc0f58e1b3a0f Mon Sep 17 00:00:00 2001 From: YusukeMizoguchi Date: Thu, 2 Jun 2022 19:07:38 +0900 Subject: [PATCH 20/23] update flowchart Signed-off-by: YusukeMizoguchi Signed-off-by: swift_file --- .../docs/image/blockage_diag_flowchart.drawio.svg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3a41ffd4d4b4b..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 @@ -
input pointcloud
input pointcloud
Depth map
Depth map
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 re...
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 +
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 From e013c4789da65df56159fbedd848cf3985bb6125 Mon Sep 17 00:00:00 2001 From: swift_file Date: Wed, 22 Jun 2022 11:39:35 +0900 Subject: [PATCH 21/23] fix: delete unnecessary item of Diagnotics Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 2 -- 1 file changed, 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 9a470a63d93be..441aac2d5d3aa 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -75,8 +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]) + "]"); - stat.add("buffering_frames", std::to_string(buffering_frames_)); - stat.add("buffering_interval", std::to_string(buffering_interval_)); // TODO(badai-nguyen): consider sky_blockage_ratio_ for DiagnosticsStatus." [todo] auto level = DiagnosticStatus::OK; From 870e577eec4314e8014dbccc1dbaf1751b17418e Mon Sep 17 00:00:00 2001 From: swift_file Date: Wed, 22 Jun 2022 16:08:58 +0900 Subject: [PATCH 22/23] update:calc blockage_ratio by multi frame blockage mask Signed-off-by: swift_file --- .../src/blockage_diag/blockage_diag_nodelet.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 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 441aac2d5d3aa..d1650a9d5f209 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -159,12 +159,7 @@ void BlockageDiagComponent::filter( cv::Point(erode_kernel_, erode_kernel_)); cv::erode(no_return_mask, erosion_dst, element); cv::dilate(erosion_dst, no_return_mask, element); - 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( - cv::Rect(0, horizontal_ring_id_, horizontal_bins, vertical_bins - horizontal_ring_id_)) - .copyTo(ground_no_return_mask); + static boost::circular_buffer no_return_mask_buffer(buffering_frames_); @@ -191,6 +186,12 @@ void BlockageDiagComponent::filter( } else { no_return_mask.copyTo(no_return_mask_result); } + cv::Mat ground_no_return_mask; + cv::Mat sky_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_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); From 756ecd459f04b7d49a2b6c5ee985b9d7149563b9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Jun 2022 10:03:54 +0000 Subject: [PATCH 23/23] ci(pre-commit): autofix --- .../pointcloud_preprocessor/docs/blockage_diag.md | 2 +- .../src/blockage_diag/blockage_diag_nodelet.cpp | 15 +++++++-------- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 9b6c0456dfdf7..6c51755768320 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -45,7 +45,7 @@ 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_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/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index d1650a9d5f209..0f740eee6be40 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -160,7 +160,6 @@ void BlockageDiagComponent::filter( 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)); @@ -186,12 +185,13 @@ void BlockageDiagComponent::filter( } else { no_return_mask.copyTo(no_return_mask_result); } - cv::Mat ground_no_return_mask; - cv::Mat sky_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); + cv::Mat ground_no_return_mask; + cv::Mat sky_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_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(horizontal_bins * (vertical_bins - horizontal_ring_id_)); @@ -237,7 +237,6 @@ void BlockageDiagComponent::filter( cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", blockage_mask_colorized).toImageMsg(); blockage_mask_msg->header = input->header; blockage_mask_pub_.publish(blockage_mask_msg); - } tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg;