From 44e0535d82cd333134c7404e9ddf9811adde74e7 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Thu, 22 Aug 2024 17:50:05 +0900 Subject: [PATCH] refactor(autoware_pointcloud_preprocessor): rework lanelet2 map filter parameters (#8491) * feat: rework lanelet2 map filter parameters Signed-off-by: vividf * chore: remove unrelated files Signed-off-by: vividf * fix: fix node name in launch Signed-off-by: vividf * chore: fix launcher Signed-off-by: vividf * chore: fix spell error Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../lanelet2_map_filter_node.param.yaml | 4 ++ .../docs/vector-map-filter.md | 5 +-- ...delet.hpp => lanelet2_map_filter_node.hpp} | 8 ++-- .../lanelet2_map_filter_node.launch.xml | 12 ++++++ .../lanelet2_map_filter_node.schema.json | 40 +++++++++++++++++++ ...delet.cpp => lanelet2_map_filter_node.cpp} | 8 ++-- 7 files changed, 66 insertions(+), 13 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/{lanelet2_map_filter_nodelet.hpp => lanelet2_map_filter_node.hpp} (96%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/{lanelet2_map_filter_nodelet.cpp => lanelet2_map_filter_node.cpp} (98%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 60c082cba53d..56c0238f104a 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -78,7 +78,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/passthrough_filter/passthrough_filter_uint16_nodelet.cpp src/passthrough_filter/passthrough_uint16.cpp src/pointcloud_accumulator/pointcloud_accumulator_nodelet.cpp - src/vector_map_filter/lanelet2_map_filter_nodelet.cpp + src/vector_map_filter/lanelet2_map_filter_node.cpp src/distortion_corrector/distortion_corrector.cpp src/distortion_corrector/distortion_corrector_node.cpp src/blockage_diag/blockage_diag_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml new file mode 100644 index 000000000000..2aae4c5e886e --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/lanelet2_map_filter_node.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + voxel_size_x: 0.04 + voxel_size_y: 0.04 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md index c38a4c719fcf..8e978adb8165 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/vector-map-filter.md @@ -25,10 +25,7 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Core Parameters -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ----------- | -| `voxel_size_x` | double | 0.04 | voxel size | -| `voxel_size_y` | double | 0.04 | voxel size | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp similarity index 96% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp index 4ba773ed618a..5572a9a6f458 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_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__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ #include #include @@ -97,4 +97,4 @@ class Lanelet2MapFilterComponent : public rclcpp::Node } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__LANELET2_MAP_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml new file mode 100644 index 000000000000..745bda311940 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/lanelet2_map_filter_node.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json new file mode 100644 index 000000000000..be7c90b5fd4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/lanelet2_map_filter_node.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Lanelet2 Map Filter Node", + "type": "object", + "definitions": { + "lanelet2_map_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along x-axis [m]", + "default": "0.04", + "minimum": 0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along y-axis [m]", + "default": "0.04", + "minimum": 0 + } + }, + "required": ["voxel_size_x", "voxel_size_y"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/lanelet2_map_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp similarity index 98% rename from sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_node.cpp index b6de8bb87d5b..ee788a2fc807 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_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/vector_map_filter/lanelet2_map_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_node.hpp" #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -39,8 +39,8 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set parameters { - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); } // Set publisher