feat: add blockage diagnostics (#461)

* feat!: add blockage diagnostic Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: add documentation Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: add adjustable param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * feat!: add blockage diagnostic Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: add documentation Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * fix: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: add adjustable param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * ci(pre-commit): autofix * chore: rearrange header file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: fix typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: rearrange header Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: revert accident change Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: fix typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: add limits Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: check overflow Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Mar 28, 2022 · 2f3e335 · 2f3e335
1 parent 7168952
commit 2f3e335
Show file tree

Hide file tree

Showing 15 changed files with 457 additions and 47 deletions.
diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt
@@ -14,6 +14,12 @@ endif()
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
+include_directories(
+  include
+  SYSTEM
+  ${PCL_INCLUDE_DIRS}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp
@@ -17,6 +17,8 @@
 
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
+#include <pcl/point_types.h>
+
 #include <cmath>
 #include <tuple>
 
@@ -41,6 +43,17 @@ struct PointXYZI
   }
 };
 
+enum ReturnType : uint8_t {
+  INVALID = 0,
+  SINGLE_STRONGEST,
+  SINGLE_LAST,
+  DUAL_STRONGEST_FIRST,
+  DUAL_STRONGEST_LAST,
+  DUAL_WEAK_FIRST,
+  DUAL_WEAK_LAST,
+  DUAL_ONLY,
+};
+
 struct PointXYZIRADRT
 {
   float x{0.0F};
@@ -77,4 +90,10 @@ using PointXYZIRADRTGenerator = std::tuple<
 
 }  // namespace autoware_point_types
 
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+  autoware_point_types::PointXYZIRADRT,
+  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
+    float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
+    double, time_stamp, time_stamp))
+
 #endif  // AUTOWARE_POINT_TYPES__TYPES_HPP_
diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml
@@ -18,6 +18,7 @@
   <depend>ament_cmake_cppcheck</depend>
   <depend>ament_cmake_lint_cmake</depend>
   <depend>ament_cmake_xmllint</depend>
+  <depend>pcl_ros</depend>
   <depend>point_cloud_msg_wrapper</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt
@@ -57,6 +57,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp
   src/vector_map_filter/lanelet2_map_filter_nodelet.cpp
   src/distortion_corrector/distortion_corrector.cpp
+  src/blockage_diag/blockage_diag_nodelet.cpp
 )
 
 target_link_libraries(pointcloud_preprocessor_filter
@@ -149,6 +150,10 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::DistortionCorrectorComponent"
   EXECUTABLE distortion_corrector_node)
 
+# ========== Blockage Diagnostics ===========
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::BlockageDiagComponent"
+  EXECUTABLE blockage_diag_node)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md
@@ -0,0 +1,60 @@
+# blockage_diag
+
+## Purpose
+
+To ensure the performance of LiDAR and safety for autonomous driving, the abnormal condition diagnostics feature is needed.
+LiDAR blockage is abnormal condition of LiDAR when some unwanted objects stitch to and block the light pulses and return signal.
+This node's purpose is to detect the existing of blockage on LiDAR and its related size and location.
+
+## Inner-workings / Algorithms
+
+This node bases on the no-return region and its location to decide if it is a blockage.
+
+![blockage situation](./image/blockage_diag.png)
+
+The logic is showed as below
+
+![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg)
+
+## Inputs / Outputs
+
+This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
+
+### Input
+
+| Name                        | Type                            | Description                                                     |
+| --------------------------- | ------------------------------- | --------------------------------------------------------------- |
+| `~/input/pointcloud_raw_ex` | `sensor_msgs::msg::PointCloud2` | The raw point cloud data is used to detect the no-return region |
+
+### Output
+
+| Name                                                 | Type                                    | Description                                        |
+| ---------------------------------------------------- | --------------------------------------- | -------------------------------------------------- |
+| `~/output/blockage_diag/debug/blockage_mask_image`   | `sensor_msgs::msg::Image`               | The mask image of detected blockage                |
+| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region |
+| `~/output/blockage_diag/debug/sky_blockage_ratio`    | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region    |
+| `~/output/blockage_diag/debug/lidar_depth_map`       | `sensor_msgs::msg::Image`               | The depth map image of input point cloud           |
+
+## Parameters
+
+| Name                       | Type   | Description                                        |
+| -------------------------- | ------ | -------------------------------------------------- |
+| `blockage_ratio_threshold` | float  | The threshold of blockage area ratio               |
+| `blockage_count_threshold` | float  | The threshold of number continuous blockage frames |
+| `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                                    |
+
+## Assumptions / Known limits
+
+1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is neccessary to check order of channel id in vertical distribution manually and modifiy the code.
+2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed.
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## References/External links
+
+## (Optional) Future extensions / Unimplemented parts
diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md
@@ -10,7 +10,7 @@ This node can remove rain and fog by considering the light reflected from the ob
 
 ![outlier_filter-return_type](./image/outlier_filter-return_type.drawio.svg)
 
-Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADT](https://github.com/tier4/AutowareArchitectureProposal.iv/blob/5d8dff0db51634f0c42d2a3e87ca423fbee84348/sensing/preprocessor/pointcloud/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp#L86-L96) data structure.
+Therefore, in order to use this node, the sensor driver must publish custom data including `return_type`. please refer to [PointXYZIRADRT](../../../common/autoware_point_types/include/autoware_point_types/types.hpp#L57-L76) data structure.
 
 Another feature of this node is that it publishes visibility as a diagnostic topic. With this function, for example, in heavy rain, the sensing module can notify that the processing performance has reached its limit, which can lead to ensuring the safety of the vehicle.
 
@@ -73,7 +73,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 ## Assumptions / Known limits
 
 Not recommended for use as it is under development.
-Input data must be `PointXYZIRADT` type data including `return_type`.
+Input data must be `PointXYZIRADRT` type data including `return_type`.
 
 ## (Optional) Error detection and handling
 

diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag.png b/sensing/pointcloud_preprocessor/docs/image/blockage_diag.png
diff --git a/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg b/sensing/pointcloud_preprocessor/docs/image/blockage_diag_flowchart.drawio.svg
diff --git a/...pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg b/...pointcloud_preprocessor/docs/image/outlier_filter-dual_return_detail.drawio.svg
diff --git a/...ointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg b/...ointcloud_preprocessor/docs/image/outlier_filter-dual_return_overall.drawio.svg
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
@@ -0,0 +1,80 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_
+#define POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_
+
+#include "pointcloud_preprocessor/filter.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <image_transport/image_transport.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <tier4_debug_msgs/msg/float32_stamped.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <string>
+#include <vector>
+
+namespace pointcloud_preprocessor
+{
+using diagnostic_updater::DiagnosticStatusWrapper;
+using diagnostic_updater::Updater;
+
+class BlockageDiagComponent : public pointcloud_preprocessor::Filter
+{
+protected:
+  virtual void filter(
+    const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
+    PointCloud2 & output);
+  /** \brief Parameter service callback result : needed to be hold */
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+
+  /** \brief Parameter service callback */
+  rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
+  image_transport::Publisher lidar_depth_map_pub_;
+  image_transport::Publisher blockage_mask_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr ground_blockage_ratio_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr sky_blockage_ratio_pub_;
+
+private:
+  void onBlockageChecker(DiagnosticStatusWrapper & stat);
+  Updater updater_{this};
+  uint vertical_bins_;
+  std::vector<double> angle_range_deg_;
+  uint horizontal_ring_id_ = 12;
+  float blockage_ratio_threshold_;
+  float ground_blockage_ratio_ = -1.0f;
+  float sky_blockage_ratio_ = -1.0f;
+  std::vector<float> ground_blockage_range_deg_ = {0.0f, 0.0f};
+  std::vector<float> sky_blockage_range_deg_ = {0.0f, 0.0f};
+  uint erode_kernel_ = 10;
+  uint ground_blockage_count_ = 0;
+  uint sky_blockage_count_ = 0;
+  uint blockage_count_threshold_;
+  std::string lidar_model_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  explicit BlockageDiagComponent(const rclcpp::NodeOptions & options);
+};
+
+}  // namespace pointcloud_preprocessor
+
+#endif  // POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_
diff --git a/...sor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/...sor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp
@@ -39,17 +39,6 @@ namespace pointcloud_preprocessor
 using diagnostic_updater::DiagnosticStatusWrapper;
 using diagnostic_updater::Updater;
 
-enum ReturnType : uint8_t {
-  INVALID = 0,
-  SINGLE_STRONGEST,
-  SINGLE_LAST,
-  DUAL_STRONGEST_FIRST,
-  DUAL_STRONGEST_LAST,
-  DUAL_WEAK_FIRST,
-  DUAL_WEAK_LAST,
-  DUAL_ONLY,
-};
-
 std::unordered_map<std::string, uint8_t> roi_mode_map_ = {
   {"No_ROI", 0},
   {"Fixed_xyz_ROI", 1},
@@ -101,26 +90,4 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter
 
 }  // namespace pointcloud_preprocessor
 
-namespace return_type_cloud
-{
-struct PointXYZIRADT
-{
-  PCL_ADD_POINT4D;
-  float intensity;
-  std::uint16_t ring;
-  float azimuth;
-  float distance;
-  std::uint8_t return_type;
-  double time_stamp;
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-} EIGEN_ALIGN16;
-
-}  // namespace return_type_cloud
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-  return_type_cloud::PointXYZIRADT,
-  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)(
-    float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)(
-    double, time_stamp, time_stamp))
-
 #endif  // POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_
diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml
@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input_topic_name" default="pointcloud_raw_ex" />
+  <arg name="output_topic_name" default="blockage_diag/pointcloud" />
+
+  <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)" />
+  </node>
+</launch>