From 5c5e9b2be1d58dad992b610e1493e5f85ebf6d49 Mon Sep 17 00:00:00 2001 From: vividf Date: Thu, 15 Aug 2024 00:18:59 +0900 Subject: [PATCH 1/2] feat: rework voxel grid outlier filter parameters Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../voxel_grid_outlier_filter_node.param.yaml | 6 +++ .../docs/voxel-grid-outlier-filter.md | 7 +-- ...hpp => voxel_grid_outlier_filter_node.hpp} | 8 ++-- .../voxel_grid_outlier_filter_node.launch.xml | 16 +++++++ ...voxel_grid_outlier_filter_node.schema.json | 48 +++++++++++++++++++ ...cpp => voxel_grid_outlier_filter_node.cpp} | 12 ++--- 7 files changed, 82 insertions(+), 17 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{voxel_grid_outlier_filter_nodelet.hpp => voxel_grid_outlier_filter_node.hpp} (92%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{voxel_grid_outlier_filter_nodelet.cpp => voxel_grid_outlier_filter_node.cpp} (90%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 744bac480e058..41867b02fc947 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -71,7 +71,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp src/outlier_filter/ring_outlier_filter_nodelet.cpp - src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp + src/outlier_filter/voxel_grid_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp src/passthrough_filter/passthrough_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..2ff4c64d5f835 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/voxel_grid_outlier_filter_node.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 + voxel_points_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md index d99b54ef09a73..920d550c2426b 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/voxel-grid-outlier-filter.md @@ -23,12 +23,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------ | ------ | ------------- | ------------------------------------------ | -| `voxel_size_x` | double | 0.3 | the voxel size along x-axis [m] | -| `voxel_size_y` | double | 0.3 | the voxel size along y-axis [m] | -| `voxel_size_z` | double | 0.1 | the voxel size along z-axis [m] | -| `voxel_points_threshold` | int | 2 | the minimum number of points in each voxel | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp index 4ec50c53c2393..c9c6d1fd2dbad 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 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__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -50,4 +50,4 @@ class VoxelGridOutlierFilterComponent : public autoware::pointcloud_preprocessor }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__VOXEL_GRID_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..380a46eed3159 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/voxel_grid_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..2131dcff0ade4 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json @@ -0,0 +1,48 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Voxel Grid Outlier Filter Node", + "type": "object", + "definitions": { + "voxel_grid_outlier_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "the voxel size along x-axis [m]", + "default": "0.3" + }, + "voxel_size_y": { + "type": "number", + "description": "the voxel size along y-axis [m]", + "default": "0.3" + }, + "voxel_size_z": { + "type": "number", + "description": "the voxel size along z-axis [m]", + "default": "0.1" + }, + "voxel_points_threshold": { + "type": "integer", + "description": "the minimum number of points in each voxel", + "default": "2" + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z", "voxel_points_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/voxel_grid_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp index 94dabf7ee5d9b..1f5cdc80a9c2c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/voxel_grid_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 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/voxel_grid_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/voxel_grid_outlier_filter_node.hpp" #include #include @@ -28,10 +28,10 @@ VoxelGridOutlierFilterComponent::VoxelGridOutlierFilterComponent( { // set initial parameters { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); - voxel_points_threshold_ = static_cast(declare_parameter("voxel_points_threshold", 2)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + voxel_points_threshold_ = declare_parameter("voxel_points_threshold"); } using std::placeholders::_1; From 492a107b404540fc7457ddf02cf930bf0c5d3c86 Mon Sep 17 00:00:00 2001 From: vividf Date: Tue, 20 Aug 2024 17:55:46 +0900 Subject: [PATCH 2/2] chore: add boundary Signed-off-by: vividf --- .../voxel_grid_outlier_filter_node.schema.json | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json index 2131dcff0ade4..586b577f8540d 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/voxel_grid_outlier_filter_node.schema.json @@ -9,22 +9,26 @@ "voxel_size_x": { "type": "number", "description": "the voxel size along x-axis [m]", - "default": "0.3" + "default": "0.3", + "minimum": 0 }, "voxel_size_y": { "type": "number", "description": "the voxel size along y-axis [m]", - "default": "0.3" + "default": "0.3", + "minimum": 0 }, "voxel_size_z": { "type": "number", "description": "the voxel size along z-axis [m]", - "default": "0.1" + "default": "0.1", + "minimum": 0 }, "voxel_points_threshold": { "type": "integer", "description": "the minimum number of points in each voxel", - "default": "2" + "default": "2", + "minimum": 1 } }, "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z", "voxel_points_threshold"],