diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index f39e45d4ea72e..11d774bffbccf 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -64,7 +64,7 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/concatenate_data/concatenate_and_time_sync_nodelet.cpp src/concatenate_data/concatenate_pointclouds.cpp - src/time_synchronizer/time_synchronizer_nodelet.cpp + src/time_synchronizer/time_synchronizer_node.cpp src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_node.cpp src/downsample_filter/random_downsample_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml new file mode 100644 index 0000000000000..5d38186840057 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_pointclouds.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + output_frame: base_link + input_topics: [ + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/top/pointcloud_before_sync" + ] + max_queue_size: 5 + timeout_sec: 0.033 + input_offset: [0.0 ,0.0 ,0.0] diff --git a/sensing/autoware_pointcloud_preprocessor/config/time_synchronizer_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/time_synchronizer_node.param.yaml new file mode 100644 index 0000000000000..9add0be3f8809 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/time_synchronizer_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + input_twist_topic_type: twist + output_frame: base_link + keep_input_frame_in_synchronized_pointcloud: false + input_topics: [ + "/sensing/lidar/left/pointcloud_before_sync", + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/top/pointcloud_before_sync" + ] + synchronized_pointcloud_postfix: pointcloud + max_queue_size: 5 + timeout_sec: 0.033 + input_offset: [0.0, 0.0, 0.0] diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index b982553a8c920..9711025b713df 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 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. diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp similarity index 98% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index 4f08ae3a3ce67..ad5cde3056ef0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 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. @@ -49,8 +49,8 @@ * */ -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_ #include #include @@ -182,4 +182,4 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/concatenate_pointcloud.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/concatenate_pointcloud.launch.xml new file mode 100644 index 0000000000000..e4fbc68ea4dd8 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/concatenate_pointcloud.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/launch/time_synchronizer_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/time_synchronizer_node.launch.xml new file mode 100644 index 0000000000000..aeb9233837e3b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/time_synchronizer_node.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/concatenate_pointclouds.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_pointclouds.schema.json new file mode 100644 index 0000000000000..916650c69a68b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_pointclouds.schema.json @@ -0,0 +1,58 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Concatenate Pointclouds Node", + "type": "object", + "definitions": { + "concatenate_pointclouds": { + "type": "object", + "properties": { + "timeout_sec": { + "type": "number", + "default": "0.1", + "minimum": 0, + "description": "Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed." + }, + "input_offset": { + "type": "array", + "items": { + "type": "number" + }, + "default": [], + "description": "This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers." + }, + "output_frame": { + "type": "string", + "default": "base_link", + "description": "Output frame id." + }, + "input_topics": { + "type": "array", + "items": { + "type": "string" + }, + "default": [], + "description": "List of input topics." + }, + "max_queue_size": { + "type": "integer", + "default": "5", + "minimum": 1, + "description": "Max queue size of input/output topics." + } + }, + "required": ["timeout_sec", "input_offset", "output_frame", "input_topics", "max_queue_size"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/concatenate_pointclouds" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/autoware_pointcloud_preprocessor/schema/time_synchronizer_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/time_synchronizer_node.schema.json new file mode 100644 index 0000000000000..8954fcaf6d814 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/time_synchronizer_node.schema.json @@ -0,0 +1,83 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Time synchronizer Node", + "type": "object", + "definitions": { + "time_synchronizer": { + "type": "object", + "properties": { + "timeout_sec": { + "type": "number", + "default": "0.033", + "minimum": 0, + "description": "Tolerance of time to publish the next pointcloud [s]. When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed." + }, + "input_offset": { + "type": "array", + "items": { + "type": "number" + }, + "default": [], + "description": "This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers." + }, + "input_twist_topic_type": { + "type": "string", + "enum": ["twist", "odom"], + "default": "twist", + "description": "Topic type for twist. Currently supports 'twist' or 'odom'." + }, + "output_frame": { + "type": "string", + "default": "base_link", + "description": "Output frame." + }, + "keep_input_frame_in_synchronized_pointcloud": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if input frame should be kept in synchronized point cloud." + }, + "input_topics": { + "type": "array", + "items": { + "type": "string" + }, + "default": [], + "description": "List of input topics." + }, + "synchronized_pointcloud_postfix": { + "type": "string", + "default": "pointcloud", + "description": "Postfix for the synchronized point cloud." + }, + "max_queue_size": { + "type": "integer", + "default": "5", + "minimum": 1, + "description": "Max queue size of input/output topics." + } + }, + "required": [ + "timeout_sec", + "input_offset", + "input_twist_topic_type", + "output_frame", + "keep_input_frame_in_synchronized_pointcloud", + "input_topics", + "synchronized_pointcloud_postfix", + "max_queue_size" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/time_synchronizer" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 2c5d7f24cb80c..e9078cbe717cb 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 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. @@ -49,12 +49,12 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( // Set parameters { - output_frame_ = static_cast(declare_parameter("output_frame", "")); + output_frame_ = declare_parameter("output_frame"); if (output_frame_.empty()) { RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); return; } - declare_parameter("input_topics", std::vector()); + declare_parameter>("input_topics"); input_topics_ = get_parameter("input_topics").as_string_array(); if (input_topics_.empty()) { RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); @@ -66,11 +66,11 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( } // Optional parameters - maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + maximum_queue_size_ = declare_parameter("max_queue_size"); /** input pointclouds should be */ - timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.033)); + timeout_sec_ = declare_parameter("timeout_sec"); - input_offset_ = declare_parameter("input_offset", std::vector{}); + input_offset_ = declare_parameter>("input_offset"); if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); return; diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 121e983f152c8..3f6f37fd31720 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 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. @@ -21,7 +21,7 @@ * @author Yoshi Ri */ -#include "autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp" #include @@ -57,14 +57,14 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Set parameters std::string synchronized_pointcloud_postfix; { - output_frame_ = static_cast(declare_parameter("output_frame", "")); + output_frame_ = declare_parameter("output_frame"); keep_input_frame_in_synchronized_pointcloud_ = - static_cast(declare_parameter("keep_input_frame_in_synchronized_pointcloud", false)); + declare_parameter("keep_input_frame_in_synchronized_pointcloud"); if (output_frame_.empty() && !keep_input_frame_in_synchronized_pointcloud_) { RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); return; } - declare_parameter("input_topics", std::vector()); + declare_parameter>("input_topics"); input_topics_ = get_parameter("input_topics").as_string_array(); if (input_topics_.empty()) { RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); @@ -76,13 +76,12 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } // output topic name postfix synchronized_pointcloud_postfix = - declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); + declare_parameter("synchronized_pointcloud_postfix"); // Optional parameters - maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); - timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); - - input_offset_ = declare_parameter("input_offset", std::vector{}); + maximum_queue_size_ = declare_parameter("max_queue_size"); + timeout_sec_ = declare_parameter("timeout_sec"); + input_offset_ = declare_parameter>("input_offset"); // If input_offset_ is not defined, set all offsets to 0 if (input_offset_.empty()) {