Merge pull request #1218 from tier4/cherry-pick/pr5932+6645

fix(blockage_diag): add dust detection enable option pr5932+6645
tier4 · Apr 1, 2024 · 009eeb5 · 009eeb5
2 parents 90c47af + afff942
commit 009eeb5
Show file tree

Hide file tree

Showing 6 changed files with 197 additions and 168 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,16 @@
+/**:
+  ros__parameters:
+    blockage_ratio_threshold: 0.1
+    blockage_count_threshold: 50
+    blockage_buffering_frames: 2
+    blockage_buffering_interval: 1
+    enable_dust_diag: false
+    publish_debug_image: false
+    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,15 +82,21 @@ 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_;
+  bool enable_dust_diag_;
+  bool publish_debug_image_;
   int dust_kernel_size_;
   int dust_buffering_frames_;
   int dust_buffering_interval_;
   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>
-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 @@