refactor(pointcloud_preprocessor): blockage_diag (autowarefoundation#…

…5932) * chore(blockage_diag): refactor Signed-off-by: badai-nguyen <[email protected]> * chore: add error logger Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * docs: update readme Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * fix: break interval angle as continuous one Signed-off-by: badai-nguyen <[email protected]> * chore: update param file Signed-off-by: badai-nguyen <[email protected]> * docs: update readme Signed-off-by: badai-nguyen <[email protected]> * chore: typo Signed-off-by: badai-nguyen <[email protected]> * fix: multi-blockage-diag in 1 container died bug fix Signed-off-by: badai-nguyen <[email protected]> * chore: update param_file Signed-off-by: badai-nguyen <[email protected]> * fix: Nan sky_blockage_ratio Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
tier4 · Mar 29, 2024 · ce8758e · ce8758e
1 parent 90c47af
commit ce8758e
Show file tree

Hide file tree

Showing 6 changed files with 73 additions and 64 deletions.
diff --git 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
@@ -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
@@ -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/...loud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/...loud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp
@@ -33,6 +33,8 @@
 #include <cv_bridge/cv_bridge.h>
 #endif
 
+#include <boost/circular_buffer.hpp>
+
 #include <string>
 #include <vector>
 
@@ -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<cv::Mat> no_return_mask_buffer{1};
+  boost::circular_buffer<cv::Mat> 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
@@ -1,37 +1,18 @@
 <launch>
   <arg name="input_topic_name" default="pointcloud_raw_ex"/>
   <arg name="output_topic_name" default="blockage_diag/pointcloud"/>
-
   <arg name="horizontal_ring_id" default="18"/>
   <arg name="angle_range" default="[0.0, 360.0]"/>
   <arg name="vertical_bins" default="40"/>
-  <arg name="model" default="Pandar40P"/>
-  <arg name="blockage_ratio_threshold" default="0.1"/>
-  <arg name="blockage_count_threshold" default="50"/>
-  <arg name="blockage_buffering_frames" default="100"/>
-  <arg name="blockage_buffering_interval" default="5"/>
-  <arg name="dust_ratio_threshold" default="0.2"/>
-  <arg name="dust_count_threshold" default="10"/>
-  <arg name="dust_kernel_size" default="2"/>
-  <arg name="dust_buffering_frames" default="10"/>
-  <arg name="dust_buffering_interval" default="1"/>
-
+  <arg name="is_channel_order_top2down" default="true"/>
+  <arg name="blockage_diagnostics_param_file" default="$(find-pkg-share pointcloud_preprocessor)/config/blockage_diagnostics_param_file.yaml"/>
   <node pkg="pointcloud_preprocessor" exec="blockage_diag_node" name="blockage_diag">
     <remap from="input" to="$(var input_topic_name)"/>
     <remap from="output" to="$(var output_topic_name)"/>
-
     <param name="horizontal_ring_id" value="$(var horizontal_ring_id)"/>
     <param name="angle_range" value="$(var angle_range)"/>
     <param name="vertical_bins" value="$(var vertical_bins)"/>
-    <param name="model" value="$(var model)"/>
-    <param name="blockage_ratio_threshold" value="$(var blockage_ratio_threshold)"/>
-    <param name="blockage_count_threshold" value="$(var blockage_count_threshold)"/>
-    <param name="blockage_buffering_frames" value="$(var blockage_buffering_frames)"/>
-    <param name="blockage_buffering_interval" value="$(var blockage_buffering_interval)"/>
-    <param name="dust_ratio_threshold" value="$(var dust_ratio_threshold)"/>
-    <param name="dust_count_threshold" value="$(var dust_count_threshold)"/>
-    <param name="dust_kernel_size" value="$(var dust_kernel_size)"/>
-    <param name="dust_buffering_frames" value="$(var dust_buffering_frames)"/>
-    <param name="dust_buffering_interval" value="$(var dust_buffering_interval)"/>
+    <param name="is_channel_order_top2down" value="$(var is_channel_order_top2down)"/>
+    <param from="$(var blockage_diagnostics_param_file)"/>
   </node>
 </launch>
diff --git 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 <boost/circular_buffer.hpp>
-
 #include <algorithm>
 #include <numeric>
 
@@ -35,7 +33,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
     blockage_ratio_threshold_ = declare_parameter<float>("blockage_ratio_threshold");
     vertical_bins_ = declare_parameter<int>("vertical_bins");
     angle_range_deg_ = declare_parameter<std::vector<double>>("angle_range");
-    lidar_model_ = declare_parameter<std::string>("model");
+    is_channel_order_top2down_ = declare_parameter<bool>("is_channel_order_top2down");
     blockage_count_threshold_ = declare_parameter<int>("blockage_count_threshold");
     blockage_buffering_frames_ = declare_parameter<int>("blockage_buffering_frames");
     blockage_buffering_interval_ = declare_parameter<int>("blockage_buffering_interval");
@@ -44,6 +42,17 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options
     dust_kernel_size_ = declare_parameter<int>("dust_kernel_size");
     dust_buffering_frames_ = declare_parameter<int>("dust_buffering_frames");
     dust_buffering_interval_ = declare_parameter<int>("dust_buffering_interval");
+    max_distance_range_ = declare_parameter<double>("max_distance_range");
+    horizontal_resolution_ = declare_parameter<double>("horizontal_resolution");
+    blockage_kernel_ = declare_parameter<int>("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<uint>((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_);
+  ideal_horizontal_bins = static_cast<int>(
+    (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_);
   pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_input(new pcl::PointCloud<PointXYZIRADRT>);
   pcl::fromROSMsg(*input, *pcl_input);
-  std::vector<float> horizontal_bin_reference(ideal_horizontal_bins);
-  std::vector<pcl::PointCloud<PointXYZIRADRT>> 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<int>(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<uint16_t>(p.ring, horizontal_bin) =
-              UINT16_MAX - distance_coefficient * p.distance;
-          } else if (lidar_model_ == "PandarQT") {
-            full_size_depth_map.at<uint16_t>(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<int>(current_angle_range / horizontal_resolution_) %
+                                   static_cast<int>(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<uint16_t>(p.ring, horizontal_bin_index) = depth_intensity;
+        } else {
+          full_size_depth_map.at<uint16_t>(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<cv::Mat> 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<float>(cv::countNonZero(ground_no_return_mask)) /
     static_cast<float>(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_));
-  sky_blockage_ratio_ = static_cast<float>(cv::countNonZero(sky_no_return_mask)) /
-                        static_cast<float>(ideal_horizontal_bins * horizontal_ring_id_);
+
+  if (horizontal_ring_id_ == 0) {
+    sky_blockage_ratio_ = 0.0f;
+  } else {
+    sky_blockage_ratio_ = static_cast<float>(cv::countNonZero(sky_no_return_mask)) /
+                          static_cast<float>(ideal_horizontal_bins * horizontal_ring_id_);
+  }
 
   if (ground_blockage_ratio_ > blockage_ratio_threshold_) {
     cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask);
@@ -295,7 +300,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<cv::Mat> 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(
@@ -405,8 +409,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(
-Original file line number
+Diff line change
@@ Expand Up / @@ -211,6 +211,7 @@ ament_export_targets(export_${PROJECT_NAME}) @@
     ament_auto_package(INSTALL_TO_SHARE
       launch
+      config
     )
@@ Expand Down @@