diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 87f198e3f48ae..2c93731972dc8 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -73,7 +73,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp - src/outlier_filter/dual_return_outlier_filter_nodelet.cpp + src/outlier_filter/dual_return_outlier_filter_node.cpp src/passthrough_filter/passthrough_filter_node.cpp src/passthrough_filter/passthrough_filter_uint16_node.cpp src/passthrough_filter/passthrough_uint16.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..5454176d7f319 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 + min_azimuth_deg: 135.0 + max_azimuth_deg: 225.0 + max_distance: 12.0 + vertical_bins: 128 + max_azimuth_diff: 50.0 + weak_first_distance_ratio: 1.004 + general_distance_ratio: 1.03 + weak_first_local_noise_threshold: 2 + roi_mode: "Fixed_xyz_ROI" + visibility_error_threshold: 0.5 + visibility_warn_threshold: 0.7 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 6c9a7ed14c2eb..8f4da273861a3 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -50,25 +50,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Description | -| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `vertical_bins` | int | The number of vertical bin for visibility histogram | -| `max_azimuth_diff` | float | Threshold for ring_outlier_filter | -| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | -| `general_distance_ratio` | double | Threshold for ring_outlier_filter | -| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | -| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | -| `roi_mode` | string | The name of ROI mode for switching | -| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | -| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | -| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | -| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | -| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | -| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | -| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp similarity index 95% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index b8aba769b17a5..ef33e88ef5c98 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 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__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -94,5 +94,5 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml deleted file mode 100644 index c65996fbcdc65..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..d0293ca140e4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..baaa0fa1f4604 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json @@ -0,0 +1,146 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dual Return Outlier Filter Node", + "type": "object", + "definitions": { + "dual_return_outlier_filter": { + "type": "object", + "properties": { + "x_max": { + "type": "number", + "description": "Maximum of x in meters for `Fixed_xyz_ROI` mode", + "default": "18.0" + }, + "x_min": { + "type": "number", + "description": "Minimum of x in meters for `Fixed_xyz_ROI` mode", + "default": "-12.0" + }, + "y_max": { + "type": "number", + "description": "Maximum of y in meters for `Fixed_xyz_ROI` mode", + "default": "2.0" + }, + "y_min": { + "type": "number", + "description": "Minimum of y in meters for `Fixed_xyz_ROI` mode", + "default": "-2.0" + }, + "z_max": { + "type": "number", + "description": "Maximum of z in meters for `Fixed_xyz_ROI` mode", + "default": "10.0" + }, + "z_min": { + "type": "number", + "description": "Minimum of z in meters for `Fixed_xyz_ROI` mode", + "default": "0.0" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "135.0", + "minimum": 0, + "maximum": 360 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "225.0", + "minimum": 0, + "maximum": 360 + }, + "max_distance": { + "type": "number", + "description": "The limit distance in meters for `Fixed_azimuth_ROI` mode", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bins for the visibility histogram", + "default": "128", + "minimum": 1 + }, + "max_azimuth_diff": { + "type": "number", + "description": "The azimuth difference threshold in degrees for ring_outlier_filter", + "default": "50.0", + "minimum": 0.0 + }, + "weak_first_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive weak point distances", + "default": "1.004", + "minimum": 0.0 + }, + "general_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive normal point distances", + "default": "1.03", + "minimum": 0.0 + }, + "weak_first_local_noise_threshold": { + "type": "integer", + "description": "If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out.", + "default": "2", + "minimum": 0 + }, + "roi_mode": { + "type": "string", + "description": "roi mode", + "default": "Fixed_xyz_ROI", + "enum": ["Fixed_xyz_ROI", "No_ROI", "Fixed_azimuth_ROI"] + }, + "visibility_error_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR", + "default": "0.5", + "minimum": 0.0, + "maximum": 1.0 + }, + "visibility_warn_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN", + "default": "0.7", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "x_max", + "x_min", + "y_max", + "y_min", + "z_max", + "z_min", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "max_azimuth_diff", + "weak_first_distance_ratio", + "general_distance_ratio", + "weak_first_local_noise_threshold", + "roi_mode", + "visibility_error_threshold", + "visibility_warn_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dual_return_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index 5fd3262088d83..d3f81473ed06f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 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/outlier_filter/dual_return_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -38,29 +38,24 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { - x_max_ = static_cast(declare_parameter("x_max", 18.0)); - x_min_ = static_cast(declare_parameter("x_min", -12.0)); - y_max_ = static_cast(declare_parameter("y_max", 2.0)); - y_min_ = static_cast(declare_parameter("y_min", -2.0)); - z_max_ = static_cast(declare_parameter("z_max", 10.0)); - z_min_ = static_cast(declare_parameter("z_min", 0.0)); - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); - weak_first_distance_ratio_ = - static_cast(declare_parameter("weak_first_distance_ratio", 1.004)); - general_distance_ratio_ = - static_cast(declare_parameter("general_distance_ratio", 1.03)); - - weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); - roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); - visibility_error_threshold_ = - static_cast(declare_parameter("visibility_error_threshold", 0.5)); - visibility_warn_threshold_ = - static_cast(declare_parameter("visibility_warn_threshold", 0.7)); + x_max_ = declare_parameter("x_max"); + x_min_ = declare_parameter("x_min"); + y_max_ = declare_parameter("y_max"); + y_min_ = declare_parameter("y_min"); + z_max_ = declare_parameter("z_max"); + z_min_ = declare_parameter("z_min"); + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + max_azimuth_diff_ = declare_parameter("max_azimuth_diff"); + weak_first_distance_ratio_ = declare_parameter("weak_first_distance_ratio"); + general_distance_ratio_ = declare_parameter("general_distance_ratio"); + + weak_first_local_noise_threshold_ = declare_parameter("weak_first_local_noise_threshold"); + roi_mode_ = declare_parameter("roi_mode"); + visibility_error_threshold_ = declare_parameter("visibility_error_threshold"); + visibility_warn_threshold_ = declare_parameter("visibility_warn_threshold"); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add(