Skip to content

Commit

Permalink
feat(blockage_diag): dust detection (#3212)
Browse files Browse the repository at this point in the history 
* implement about dust detection

Signed-off-by: Yusuke Mizoguchi <[email protected]>

* chore: fix spell

Signed-off-by: badai-nguyen <[email protected]>

* chore: refactor

Signed-off-by: badai-nguyen <[email protected]>

* bug fix

Signed-off-by: Shunsuke Miura <[email protected]>

* fix: avoid counter overflow

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: Yusuke Mizoguchi <[email protected]>
Signed-off-by: badai-nguyen <[email protected]>
Signed-off-by: Shunsuke Miura <[email protected]>
Co-authored-by: badai nguyen <[email protected]>
Co-authored-by: badai-nguyen <[email protected]>
Co-authored-by: Shunsuke Miura <[email protected]>
  • Loading branch information
@swiftfile @badai-nguyen @miursh
4 people authored Sep 28, 2023
1 parent ee7c345 commit 1caa1b3
Show file tree
Hide file tree
Showing 4 changed files with 362 additions and 149 deletions.
63 changes: 42 additions & 21 deletions sensing/pointcloud_preprocessor/docs/blockage_diag.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,13 @@

## 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.
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
## Inner-workings / Algorithms(Blockage detection)

This node bases on the no-return region and its location to decide if it is a blockage.

Expand All @@ -16,6 +18,14 @@ The logic is showed as below

![blockage_diag_flowchart](./image/blockage_diag_flowchart.drawio.svg)

## Inner-workings /Algorithms(Dust detection)

About dust detection, morphological processing is implemented.
If the lidar's ray cannot be acquired due to dust in the lidar area where the point cloud is considered to return from
the ground,
black pixels appear as noise in the depth image.
The area of noise is found by erosion and dilation these black pixels.

## Inputs / Outputs

This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
Expand All @@ -28,30 +38,41 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref

### 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 |
| 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 |
| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame |
| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area |
| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results |
| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. |

## 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 |
| `buffering_frames` | uint | The number of buffering [range:1-200] |
| `buffering_interval` | uint | The interval of buffering |
| Name | Type | Description |
| ----------------------------- | ------ | ----------------------------------------------------------------------------------------------------------------------------- |
| `blockage_ratio_threshold` | float | The threshold of blockage area ratio.If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR. |
| `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 |
| `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 |
| `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 |

## Assumptions / Known limits

1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in vertical distribution manually and modify the code.
2. The current method is still limited for dust type of blockage when dust particles are sparsely distributed.
1. Only Hesai Pandar40P and Hesai PandarQT were tested. For a new LiDAR, it is necessary to check order of channel id in
vertical distribution manually and modify the code.
2. About dusty area detection, False positives occur when there are water puddles on the road surface due to rain, etc.
Also, the area of the ray to the sky is currently undetectable.

## (Optional) Error detection and handling

Expand Down
31 changes: 23 additions & 8 deletions ...loud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,27 +53,42 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter
rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
image_transport::Publisher lidar_depth_map_pub_;
image_transport::Publisher blockage_mask_pub_;
image_transport::Publisher single_frame_dust_mask_pub;
image_transport::Publisher multi_frame_dust_mask_pub;
image_transport::Publisher blockage_dust_merged_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_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32Stamped>::SharedPtr ground_dust_ratio_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::StringStamped>::SharedPtr blockage_type_pub_;

private:
void onBlockageChecker(DiagnosticStatusWrapper & stat);
void dustChecker(DiagnosticStatusWrapper & stat);
Updater updater_{this};
uint vertical_bins_;
int vertical_bins_;
std::vector<double> angle_range_deg_;
uint horizontal_ring_id_ = 12;
int horizontal_ring_id_;
float blockage_ratio_threshold_;
float dust_ratio_threshold_;
float ground_blockage_ratio_ = -1.0f;
float sky_blockage_ratio_ = -1.0f;
float ground_dust_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_;
int blockage_kernel_ = 10;
int blockage_frame_count_ = 0;
int ground_blockage_count_ = 0;
int sky_blockage_count_ = 0;
int blockage_count_threshold_;
std::string lidar_model_;
uint buffering_frames_ = 100;
uint buffering_interval_ = 5;
int blockage_buffering_frames_;
int blockage_buffering_interval_;
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;

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
28 changes: 28 additions & 0 deletions sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,36 @@
<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"/>

<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)"/>
</node>
</launch>
Loading

0 comments on commit 1caa1b3

Please sign in to comment.