From 8ba6168ea1bd15cb74c3fb3ec4fa0851d91f91e7 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 21 Aug 2024 00:22:14 +0900 Subject: [PATCH] refactor(autoware_pointcloud_preprocessor): rework blockage diag parameters (#8488) * feat: rework blockage diag parameters Signed-off-by: vividf * chore: fix readme Signed-off-by: vividf * chore: fix schema description Signed-off-by: vividf * chore: add boundary for schema Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- ...ile.yaml => blockage_diag_node.param.yaml} | 4 + .../docs/blockage_diag.md | 19 +-- ...iag_nodelet.hpp => blockage_diag_node.hpp} | 8 +- .../launch/blockage_diag.launch.xml | 18 -- .../launch/blockage_diag_node.launch.xml | 10 ++ .../schema/blockage_diag_node.schema.json | 159 ++++++++++++++++++ ...iag_nodelet.cpp => blockage_diag_node.cpp} | 4 +- 8 files changed, 181 insertions(+), 43 deletions(-) rename sensing/autoware_pointcloud_preprocessor/config/{blockage_diagnostics_param_file.yaml => blockage_diag_node.param.yaml} (79%) rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/{blockage_diag_nodelet.hpp => blockage_diag_node.hpp} (97%) delete mode 100644 sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/blockage_diag_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/blockage_diag/{blockage_diag_nodelet.cpp => blockage_diag_node.cpp} (99%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 744bac480e058..2d7b179160ded 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -81,7 +81,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/vector_map_filter/lanelet2_map_filter_nodelet.cpp src/distortion_corrector/distortion_corrector.cpp src/distortion_corrector/distortion_corrector_node.cpp - src/blockage_diag/blockage_diag_nodelet.cpp + src/blockage_diag/blockage_diag_node.cpp src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter.cpp src/utility/geometry.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/autoware_pointcloud_preprocessor/config/blockage_diag_node.param.yaml similarity index 79% rename from sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml rename to sensing/autoware_pointcloud_preprocessor/config/blockage_diag_node.param.yaml index 13ddd8a7c7590..75503a3caf159 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/blockage_diag_node.param.yaml @@ -14,3 +14,7 @@ max_distance_range: 200.0 horizontal_resolution: 0.4 blockage_kernel: 10 + angle_range: [0.0, 360.0] + vertical_bins: 40 + is_channel_order_top2down: true + horizontal_ring_id: 18 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md index b6f858ada3a98..911cda3823021 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md @@ -51,24 +51,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ## Parameters -| 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 | -| `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] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp index 18f6c3851866d..3601b492c4fe3 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2024 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -105,4 +105,4 @@ class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__BLOCKAGE_DIAG__BLOCKAGE_DIAG_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml deleted file mode 100644 index 281ddd7ce434e..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag_node.launch.xml new file mode 100644 index 0000000000000..852a8af43b7d7 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/blockage_diag_node.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json new file mode 100644 index 0000000000000..0e4a02d37bd16 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json @@ -0,0 +1,159 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Blockage Diag Node", + "type": "object", + "definitions": { + "blockage_diag": { + "type": "object", + "properties": { + "blockage_ratio_threshold": { + "type": "number", + "description": "The threshold of blockage area ratio. If the blockage value exceeds this threshold, the diagnostic state will be set to ERROR.", + "default": "0.1", + "minimum": 0 + }, + "blockage_count_threshold": { + "type": "number", + "description": "The threshold of number continuous blockage frames", + "default": "50", + "minimum": 0 + }, + "blockage_buffering_frames": { + "type": "integer", + "description": "The number of buffering about blockage detection [range:1-200]", + "default": "2", + "minimum": 1, + "maximum": 200 + }, + "blockage_buffering_interval": { + "type": "integer", + "description": "The interval of buffering about blockage detection", + "default": "1", + "minimum": 0 + }, + "enable_dust_diag": { + "type": "boolean", + "description": "enable dust diagnostic", + "default": "false" + }, + "publish_debug_image": { + "type": "boolean", + "description": "publish debug image", + "default": "false" + }, + "dust_ratio_threshold": { + "type": "number", + "description": "The threshold of dusty area ratio", + "default": "0.2", + "minimum": 0 + }, + "dust_count_threshold": { + "type": "integer", + "description": "The threshold of number continuous frames include dusty area", + "default": "10", + "minimum": 0 + }, + "dust_kernel_size": { + "type": "integer", + "description": "The kernel size of morphology processing in dusty area detection", + "default": "2", + "minimum": 0 + }, + "dust_buffering_frames": { + "type": "integer", + "description": "The number of buffering about dusty area detection [range:1-200]", + "default": "10", + "minimum": 1, + "maximum": 200 + }, + "dust_buffering_interval": { + "type": "integer", + "description": "The interval of buffering about dusty area detection", + "default": "1", + "minimum": 0 + }, + "max_distance_range": { + "type": "number", + "description": "Maximum view range for the LiDAR", + "default": "200.0", + "minimum": 0 + }, + "horizontal_resolution": { + "type": "number", + "description": "The horizontal resolution of depth map image [deg/pixel]", + "default": "0.4", + "minimum": 0 + }, + "blockage_kernel": { + "type": "integer", + "description": "The kernel size of morphology processing the detected blockage area", + "default": "10", + "minimum": 0 + }, + "angle_range": { + "type": "array", + "description": "The effective range of LiDAR", + "minItems": 2, + "maxItems": 2, + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 360.0 + } + }, + "vertical_bins": { + "type": "integer", + "description": "The LiDAR channel", + "default": "40", + "minimum": 0 + }, + "is_channel_order_top2down": { + "type": "boolean", + "description": "If the lidar channels are indexed from top to down", + "default": "true" + }, + "horizontal_ring_id": { + "type": "integer", + "description": "The id of horizontal ring of the LiDAR", + "default": "18", + "minimum": 0 + } + }, + "required": [ + "blockage_ratio_threshold", + "blockage_count_threshold", + "blockage_buffering_frames", + "blockage_buffering_interval", + "enable_dust_diag", + "publish_debug_image", + "dust_ratio_threshold", + "dust_count_threshold", + "dust_kernel_size", + "dust_buffering_frames", + "dust_buffering_interval", + "max_distance_range", + "horizontal_resolution", + "blockage_kernel", + "angle_range", + "vertical_bins", + "is_channel_order_top2down", + "horizontal_ring_id" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/blockage_diag" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp similarity index 99% rename from sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 9d086e16dd51f..2f34d78b8e9a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2024 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp" #include "autoware_point_types/types.hpp"