diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt new file mode 100644 index 0000000000000..d6f7e020771a2 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/CMakeLists.txt @@ -0,0 +1,94 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_cuda_pointcloud_preprocessor) + +find_package(ament_cmake_auto REQUIRED) +find_package(CUDA) + +if(NOT ${CUDA_FOUND}) + message(WARNING "cuda was not found, so the autoware_cuda_pointcloud_preprocessor package will not be built.") + return() +endif() + +ament_auto_find_build_dependencies() + +# Default to C++17 +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wunused-function) +endif () + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories( + include + SYSTEM + ${CUDA_INCLUDE_DIRS} +) + +list(APPEND CUDA_NVCC_FLAGS "--expt-relaxed-constexpr -diag-suppress 20012") # cSpell: ignore expt + +cuda_add_library(cuda_pointcloud_preprocessor_lib SHARED + src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp + src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu + src/cuda_concatenate_data/cuda_cloud_collector.cpp + src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp +) + +target_link_libraries(cuda_pointcloud_preprocessor_lib + ${autoware_pointcloud_preprocessor_TARGETS} +) + +target_include_directories(cuda_pointcloud_preprocessor_lib SYSTEM PRIVATE + ${autoware_pointcloud_preprocessor_INCLUDE_DIRS} + ${autoware_point_types_INCLUDE_DIRS} + ${cuda_blackboard_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} + ${rcl_interfaces_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${tf2_msgs_INCLUDE_DIRS} +) + +# Targets +ament_auto_add_library(cuda_pointcloud_preprocessor SHARED + src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp +) + +target_link_libraries(cuda_pointcloud_preprocessor + ${CUDA_LIBRARIES} + + ${diagnostic_msgs_LIBRARIES} + cuda_pointcloud_preprocessor_lib +) + +# ========== Concatenate and Sync data ========== +rclcpp_components_register_node(cuda_pointcloud_preprocessor + PLUGIN "autoware::cuda_pointcloud_preprocessor::CudaPointCloudConcatenateDataSynchronizerComponent" + EXECUTABLE cuda_concatenate_and_time_sync_node) + +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_auto_package() + +# Set ROS_DISTRO macros +set(ROS_DISTRO $ENV{ROS_DISTRO}) +if(${ROS_DISTRO} STREQUAL "rolling") + add_compile_definitions(ROS_DISTRO_ROLLING) +elseif(${ROS_DISTRO} STREQUAL "foxy") + add_compile_definitions(ROS_DISTRO_FOXY) +elseif(${ROS_DISTRO} STREQUAL "galactic") + add_compile_definitions(ROS_DISTRO_GALACTIC) +elseif(${ROS_DISTRO} STREQUAL "humble") + add_compile_definitions(ROS_DISTRO_HUMBLE) +endif() diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/README.md b/sensing/autoware_cuda_pointcloud_preprocessor/README.md new file mode 100644 index 0000000000000..ceed68ca07a83 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/README.md @@ -0,0 +1,20 @@ +# autoware_cuda_pointcloud_preprocessor + +## Purpose + +The pointcloud preprocessing implemented in `autoware_pointcloud_preprocessor` has been thoroughly tested in autoware. However, the latency it introduces does not scale well with modern LiDAR devices due to the high number of points they introduce. + +To alleviate this issue, this package reimplements most of the pipeline presented in `autoware_pointcloud_preprocessor` leveraging the use of GPGPUs. In particular, this package makes use of CUDA to provide accelerated versions of the already established implementations, while also maintaining compatibility with normal ROS nodes/topics. + + + + + + + diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/package.xml b/sensing/autoware_cuda_pointcloud_preprocessor/package.xml new file mode 100644 index 0000000000000..fecb38c2f48f4 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/package.xml @@ -0,0 +1,32 @@ + + + autoware_cuda_pointcloud_preprocessor + 0.1.0 + autoware_cuda_pointcloud_preprocessor + Kenzo Lobos-Tsunekawa + Kenzo Lobos-Tsunekawa + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_point_types + autoware_pointcloud_preprocessor + autoware_universe_utils + cuda_blackboard + diagnostic_msgs + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_msgs + tier4_debug_msgs + + ament_clang_format + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json b/sensing/autoware_cuda_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json new file mode 100644 index 0000000000000..7a71b23dc841b --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json @@ -0,0 +1,163 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Cuda Concatenate and Time Synchronize Node", + "type": "object", + "definitions": { + "cuda_concatenate_and_time_sync_node": { + "type": "object", + "properties": { + "debug_mode": { + "type": "boolean", + "default": false, + "description": "Flag to enable debug mode to display additional logging information." + }, + "has_static_tf_only": { + "type": "boolean", + "default": false, + "description": "Flag to indicate if only static TF is used." + }, + "rosbag_length": { + "type": "number", + "default": 10.0, + "minimum": 0.0, + "description": " This value determine if the rosbag has started from the beginning again. The value should be set smaller than the actual length of the bag." + }, + "maximum_queue_size": { + "type": "integer", + "default": "5", + "minimum": 1, + "description": "Maximum size of the queue for the Keep Last policy in QoS history." + }, + "timeout_sec": { + "type": "number", + "default": "0.1", + "minimum": 0.001, + "description": "Timer's timeout duration in seconds when collectors are created. Collectors will concatenate the point clouds when timeout_sec reaches zero." + }, + "is_motion_compensated": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if motion compensation is enabled." + }, + "publish_synchronized_pointcloud": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if synchronized point cloud should be published." + }, + "keep_input_frame_in_synchronized_pointcloud": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if input frame should be kept in synchronized point cloud." + }, + "publish_previous_but_late_pointcloud": { + "type": "boolean", + "default": false, + "description": "Flag to indicate if a concatenated point cloud should be published if its timestamp is earlier than the previous published concatenated point cloud." + }, + "synchronized_pointcloud_postfix": { + "type": "string", + "default": "pointcloud", + "description": "Postfix for the topic name of the synchronized point cloud." + }, + "input_twist_topic_type": { + "type": "string", + "enum": ["twist", "odom"], + "default": "twist", + "description": "Type of the input twist topic." + }, + "input_topics": { + "type": "array", + "items": { + "type": "string", + "minLength": 1 + }, + "default": ["cloud_topic1", "cloud_topic2", "cloud_topic3"], + "minItems": 2, + "description": "List of input point cloud topics." + }, + "output_frame": { + "type": "string", + "default": "base_link", + "minLength": 1, + "description": "Output frame." + }, + "matching_strategy": { + "type": "object", + "properties": { + "type": { + "type": "string", + "enum": ["naive", "advanced"], + "default": "advanced", + "description": "Set it to `advanced` if you can synchronize your LiDAR sensor; otherwise, set it to `naive`." + }, + "lidar_timestamp_offsets": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0 + }, + "default": [0.0, 0.0, 0.0], + "minItems": 2, + "description": "List of LiDAR timestamp offsets in seconds (relative to the earliest LiDAR timestamp). The offsets should be provided in the same order as the input topics." + }, + "lidar_timestamp_noise_window": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0 + }, + "default": [0.01, 0.01, 0.01], + "minItems": 2, + "description": "List of LiDAR timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics." + } + }, + "required": ["type"], + "dependencies": { + "type": { + "oneOf": [ + { + "properties": { "type": { "const": "naive" } }, + "not": { + "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"] + } + }, + { + "properties": { "type": { "const": "advanced" } }, + "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"] + } + ] + } + } + } + }, + "required": [ + "debug_mode", + "has_static_tf_only", + "rosbag_length", + "maximum_queue_size", + "timeout_sec", + "is_motion_compensated", + "publish_synchronized_pointcloud", + "keep_input_frame_in_synchronized_pointcloud", + "publish_previous_but_late_pointcloud", + "synchronized_pointcloud_postfix", + "input_twist_topic_type", + "input_topics", + "output_frame", + "matching_strategy" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/cuda_concatenate_and_time_sync_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_cloud_collector.cpp b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_cloud_collector.cpp new file mode 100644 index 0000000000000..70686d6bfb1bd --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_cloud_collector.cpp @@ -0,0 +1,20 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_cloud_collector.hpp" + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp" + +template class autoware::pointcloud_preprocessor::CloudCollector< + autoware::pointcloud_preprocessor::CudaPointCloud2Traits>; diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp new file mode 100644 index 0000000000000..ca183e1f39f01 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_collector_matching_strategy.cpp @@ -0,0 +1,21 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp" + +template class autoware::pointcloud_preprocessor::NaiveMatchingStrategy< + autoware::pointcloud_preprocessor::CudaPointCloud2Traits>; +template class autoware::pointcloud_preprocessor::AdvancedMatchingStrategy< + autoware::pointcloud_preprocessor::CudaPointCloud2Traits>; diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp new file mode 100644 index 0000000000000..e954d6240a960 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler.cpp @@ -0,0 +1,265 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_combine_cloud_handler.hpp" + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.hpp" +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp" + +#include + +#include + +#include +#include +#include +#include + +#define CHECK_OFFSET(structure1, structure2, field) \ + static_assert( \ + offsetof(structure1, field) == offsetof(structure2, field), \ + "Offset of " #field " in " #structure1 " does not match expected offset.") + +static_assert( + sizeof(autoware::pointcloud_preprocessor::PointTypeStruct) == + sizeof(autoware::point_types::PointXYZIRC)); + +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, x); +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, y); +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, z); +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, + intensity); +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, + return_type); +CHECK_OFFSET( + autoware::pointcloud_preprocessor::PointTypeStruct, autoware::point_types::PointXYZIRCAEDT, + channel); + +namespace autoware::pointcloud_preprocessor +{ + +CombineCloudHandler::CombineCloudHandler( + rclcpp::Node & node, const std::vector & input_topics, std::string output_frame, + bool is_motion_compensated, bool publish_synchronized_pointcloud, + bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only) +: CombineCloudHandlerBase( + node, input_topics, output_frame, is_motion_compensated, publish_synchronized_pointcloud, + keep_input_frame_in_synchronized_pointcloud, has_static_tf_only) +{ + for (const auto & topic : input_topics_) { + CudaConcatStruct cuda_concat_struct; + cudaStreamCreate(&cuda_concat_struct.stream); + cuda_concat_struct_map_[topic] = std::move(cuda_concat_struct); + } +} + +void CombineCloudHandler::allocate_pointclouds() +{ + std::lock_guard lock(mutex_); + + for (const auto & topic : input_topics_) { + auto & concat_struct = cuda_concat_struct_map_[topic]; + concat_struct.cloud_ptr = std::make_unique(); + concat_struct.cloud_ptr->data = + cuda_blackboard::make_unique(concat_struct.max_pointcloud_size); + } + + concatenated_cloud_ptr_ = std::make_unique(); + concatenated_cloud_ptr_->data = cuda_blackboard::make_unique( + max_concat_pointcloud_size_ * input_topics_.size()); +} + +ConcatenatedCloudResult +CombineCloudHandler::combine_pointclouds( + std::unordered_map< + std::string, typename CudaPointCloud2Traits::PointCloudMessage::ConstSharedPtr> & + topic_to_cloud_map) +{ + ConcatenatedCloudResult concatenate_cloud_result; + std::lock_guard lock(mutex_); + + std::vector pc_stamps; + for (const auto & [topic, cloud] : topic_to_cloud_map) { + pc_stamps.emplace_back(cloud->header.stamp); + } + + std::sort(pc_stamps.begin(), pc_stamps.end(), std::greater()); + auto oldest_stamp = pc_stamps.back(); + + // Before combining the pointclouds, initialize and reserve space for the concatenated pointcloud + concatenate_cloud_result.concatenate_cloud_ptr = + std::make_unique(); + + // Reserve space based on the total size of the pointcloud data to speed up the concatenation + // process + size_t total_data_size = 0; + size_t total_points = 0; + for (const auto & [topic, cloud] : topic_to_cloud_map) { + total_data_size += (cloud->height * cloud->row_step); + total_points += (cloud->height * cloud->width); + } + + const auto point_fields = topic_to_cloud_map.begin()->second->fields; + + if (total_data_size > max_concat_pointcloud_size_ || !concatenated_cloud_ptr_) { + max_concat_pointcloud_size_ = (total_data_size + 1024) / 1024 * 1024; + concatenated_cloud_ptr_ = std::make_unique(); + concatenated_cloud_ptr_->data = cuda_blackboard::make_unique( + max_concat_pointcloud_size_ * input_topics_.size()); + } + + concatenate_cloud_result.concatenate_cloud_ptr = std::move(concatenated_cloud_ptr_); + + PointTypeStruct * output_points = + reinterpret_cast(concatenate_cloud_result.concatenate_cloud_ptr->data.get()); + std::size_t concatenated_start_index = 0; + + for (const auto & [topic, cloud] : topic_to_cloud_map) { + const std::size_t num_points = cloud->height * cloud->width; + + // Compute motion compensation transform + Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); + + // Transform if needed + managed_tf_buffer_->getTransform(output_frame_, cloud->header.frame_id, transform); + + rclcpp::Time current_cloud_stamp = rclcpp::Time(cloud->header.stamp); + + if (is_motion_compensated_) { + transform = compute_transform_to_adjust_for_old_timestamp(oldest_stamp, current_cloud_stamp) * + transform; + } + + TransformStruct transform_struct; + transform_struct.translation_x = transform(0, 3); + transform_struct.translation_y = transform(1, 3); + transform_struct.translation_z = transform(2, 3); + transform_struct.m11 = transform(0, 0); + transform_struct.m12 = transform(0, 1); + transform_struct.m13 = transform(0, 2); + transform_struct.m21 = transform(1, 0); + transform_struct.m22 = transform(1, 1); + transform_struct.m23 = transform(1, 2); + transform_struct.m31 = transform(2, 0); + transform_struct.m32 = transform(2, 1); + transform_struct.m33 = transform(2, 2); + + auto & stream = cuda_concat_struct_map_[topic].stream; + transform_launch( + reinterpret_cast(cloud->data.get()), num_points, transform_struct, + output_points + concatenated_start_index, stream); + concatenated_start_index += num_points; + } + + concatenate_cloud_result.concatenate_cloud_ptr->header.frame_id = output_frame_; + concatenate_cloud_result.concatenate_cloud_ptr->width = concatenated_start_index; + concatenate_cloud_result.concatenate_cloud_ptr->height = 1; + concatenate_cloud_result.concatenate_cloud_ptr->point_step = sizeof(PointTypeStruct); + concatenate_cloud_result.concatenate_cloud_ptr->row_step = + concatenated_start_index * sizeof(PointTypeStruct); + concatenate_cloud_result.concatenate_cloud_ptr->fields = point_fields; + concatenate_cloud_result.concatenate_cloud_ptr->is_bigendian = false; + concatenate_cloud_result.concatenate_cloud_ptr->is_dense = true; + + // Second round is for when we need to publish sync pointclouds + if (publish_synchronized_pointcloud_) { + if (!concatenate_cloud_result.topic_to_transformed_cloud_map) { + // Initialize the map if it is not present + concatenate_cloud_result.topic_to_transformed_cloud_map = + std::unordered_map(); + } + + concatenated_start_index = 0; + + for (const auto & [topic, cloud] : topic_to_cloud_map) { + const std::size_t num_points = cloud->height * cloud->width; + const std::size_t data_size = cloud->height * cloud->row_step; + + auto & concat_struct = cuda_concat_struct_map_[topic]; + + if (data_size > concat_struct.max_pointcloud_size || !concat_struct.cloud_ptr) { + concat_struct.max_pointcloud_size = (data_size + 1024) / 1024 * 1024; + concat_struct.cloud_ptr = std::make_unique(); + concat_struct.cloud_ptr->data = cuda_blackboard::make_unique(data_size); + } + // convert to original sensor frame if necessary + + auto & output_cloud = (*concatenate_cloud_result.topic_to_transformed_cloud_map)[topic]; + bool need_transform_to_sensor_frame = (cloud->header.frame_id != output_frame_); + + output_cloud = std::move(concat_struct.cloud_ptr); + + auto & stream = cuda_concat_struct_map_[topic].stream; + + if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { + Eigen::Matrix4f transform; + managed_tf_buffer_->getTransform(cloud->header.frame_id, output_frame_, transform); + + TransformStruct transform_struct; + transform_struct.translation_x = transform(0, 3); + transform_struct.translation_y = transform(1, 3); + transform_struct.translation_z = transform(2, 3); + transform_struct.m11 = transform(0, 0); + transform_struct.m12 = transform(0, 1); + transform_struct.m13 = transform(0, 2); + transform_struct.m21 = transform(1, 0); + transform_struct.m22 = transform(1, 1); + transform_struct.m23 = transform(1, 2); + transform_struct.m31 = transform(2, 0); + transform_struct.m32 = transform(2, 1); + transform_struct.m33 = transform(2, 2); + + transform_launch( + output_points + concatenated_start_index, num_points, transform_struct, + reinterpret_cast(output_cloud->data.get()), stream); + output_cloud->header.frame_id = cloud->header.frame_id; + } else { + cudaMemcpyAsync( + output_cloud->data.get(), output_points + concatenated_start_index, data_size, + cudaMemcpyDeviceToDevice, stream); + output_cloud->header.frame_id = output_frame_; + } + + output_cloud->header.stamp = cloud->header.stamp; + output_cloud->width = cloud->width; + output_cloud->height = cloud->height; + output_cloud->point_step = sizeof(PointTypeStruct); + output_cloud->row_step = cloud->width * sizeof(PointTypeStruct); + output_cloud->fields = point_fields; + output_cloud->is_bigendian = false; + output_cloud->is_dense = true; + + concatenated_start_index += cloud->height * cloud->width; + } + } + + // Sync all streams + for (const auto & [topic, cuda_concat_struct] : cuda_concat_struct_map_) { + cudaStreamSynchronize(cuda_concat_struct.stream); + } + + concatenate_cloud_result.concatenate_cloud_ptr->header.stamp = oldest_stamp; + + return concatenate_cloud_result; +} + +} // namespace autoware::pointcloud_preprocessor + +template class autoware::pointcloud_preprocessor::CombineCloudHandler< + autoware::pointcloud_preprocessor::CudaPointCloud2Traits>; diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu new file mode 100644 index 0000000000000..88bd1094267d6 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.cu @@ -0,0 +1,55 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_combine_cloud_handler_kernel.hpp" + +#include + +namespace autoware::pointcloud_preprocessor +{ + +__global__ void transform_kernel( + const PointTypeStruct * input_points, int num_points, TransformStruct transform, + PointTypeStruct * output_points) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx < num_points) { + float x = input_points[idx].x; + float y = input_points[idx].y; + float z = input_points[idx].z; + + output_points[idx].x = + transform.m11 * x + transform.m12 * y + transform.m13 * z + transform.translation_x; + output_points[idx].y = + transform.m21 * x + transform.m22 * y + transform.m23 * z + transform.translation_y; + output_points[idx].z = + transform.m31 * x + transform.m32 * y + transform.m33 * z + transform.translation_z; + output_points[idx].intensity = input_points[idx].intensity; + output_points[idx].return_type = input_points[idx].return_type; + output_points[idx].channel = input_points[idx].channel; + } +} + +void transform_launch( + const PointTypeStruct * input_points, int num_points, TransformStruct transform, + PointTypeStruct * output_points, cudaStream_t & stream) +{ + constexpr int threads_per_block = 256; + const int block_per_grid = (num_points + threads_per_block - 1) / threads_per_block; + + transform_kernel<<>>( + input_points, num_points, transform, output_points); +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp new file mode 100644 index 0000000000000..fd44f2b5e35c3 --- /dev/null +++ b/sensing/autoware_cuda_pointcloud_preprocessor/src/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.cpp @@ -0,0 +1,73 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_concatenate_and_time_sync_node.hpp" + +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_cloud_collector.hpp" +#include "autoware/cuda_pointcloud_preprocessor/cuda_concatenate_data/cuda_traits.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" + +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +template <> +void PointCloudConcatenateDataSynchronizerComponentTemplated::initialize() +{ + concatenated_cloud_publisher_ = + std::make_shared>( + *this, "output"); + + for (auto & topic : params_.input_topics) { + std::string new_topic = + replace_sync_topic_name_postfix(topic, params_.synchronized_pointcloud_postfix); + auto publisher = + std::make_shared>( + *this, new_topic); + topic_to_transformed_cloud_publisher_map_.insert({topic, publisher}); + } + + for (const std::string & topic : params_.input_topics) { + auto callback = [&](const cuda_blackboard::CudaPointCloud2::ConstSharedPtr msg) { + this->cloud_callback(msg, topic); + }; + + auto pointcloud_sub = + std::make_shared>( + *this, topic, false, callback); + pointcloud_subs_.push_back(pointcloud_sub); + } + + RCLCPP_DEBUG_STREAM( + get_logger(), + "Subscribing to " << params_.input_topics.size() << " user given topics as inputs:"); + + for (const auto & input_topic : params_.input_topics) { + RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); + } +} + +} // namespace autoware::pointcloud_preprocessor + +template class autoware::pointcloud_preprocessor:: + PointCloudConcatenateDataSynchronizerComponentTemplated< + autoware::pointcloud_preprocessor::CudaPointCloud2Traits>; + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::cuda_pointcloud_preprocessor::CudaPointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 19c698b11bc5f..dc5be9cf9c1b2 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -63,12 +63,32 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter sensor_msgs ) -ament_auto_add_library(pointcloud_preprocessor_filter SHARED +add_library(concatenate_data SHARED src/concatenate_data/concatenate_and_time_sync_node.cpp src/concatenate_data/combine_cloud_handler.cpp src/concatenate_data/cloud_collector.cpp src/concatenate_data/collector_matching_strategy.cpp src/concatenate_data/concatenate_pointclouds.cpp +) + +target_include_directories(concatenate_data PUBLIC + "$" + "$" +) + +ament_target_dependencies(concatenate_data + SYSTEM + pcl_conversions + rclcpp + sensor_msgs + tf2_ros + autoware_universe_utils + pcl_ros + autoware_point_types +) + +ament_auto_add_library(pointcloud_preprocessor_filter SHARED + src/crop_box_filter/crop_box_filter_node.cpp src/time_synchronizer/time_synchronizer_node.cpp src/downsample_filter/voxel_grid_downsample_filter_node.cpp @@ -95,6 +115,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base faster_voxel_grid_downsample_filter + concatenate_data ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} @@ -220,6 +241,13 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS concatenate_data EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp index 13604569df9a8..085dc495c76c0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp @@ -23,7 +23,10 @@ namespace autoware::pointcloud_preprocessor { -class PointCloudConcatenateDataSynchronizerComponent; +template +class PointCloudConcatenateDataSynchronizerComponentTemplated; + +template class CombineCloudHandler; struct CollectorInfoBase @@ -49,22 +52,25 @@ struct AdvancedCollectorInfo : public CollectorInfoBase } }; +template class CloudCollector { public: CloudCollector( - std::shared_ptr && ros2_parent_node, - std::shared_ptr & combine_cloud_handler, int num_of_clouds, + std::shared_ptr> && + ros2_parent_node, + std::shared_ptr> & combine_cloud_handler, int num_of_clouds, double timeout_sec, bool debug_mode); bool topic_exists(const std::string & topic_name); bool process_pointcloud( - const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud); + const std::string & topic_name, typename MsgTraits::PointCloudMessage::ConstSharedPtr cloud); void concatenate_callback(); - ConcatenatedCloudResult concatenate_pointclouds( - std::unordered_map topic_to_cloud_map); + ConcatenatedCloudResult concatenate_pointclouds( + std::unordered_map + topic_to_cloud_map); - std::unordered_map + std::unordered_map get_topic_to_cloud_map(); [[nodiscard]] bool concatenate_finished() const; @@ -74,10 +80,12 @@ class CloudCollector void show_debug_message(); private: - std::shared_ptr ros2_parent_node_; - std::shared_ptr combine_cloud_handler_; + std::shared_ptr> + ros2_parent_node_; + std::shared_ptr> combine_cloud_handler_; rclcpp::TimerBase::SharedPtr timer_; - std::unordered_map topic_to_cloud_map_; + std::unordered_map + topic_to_cloud_map_; uint64_t num_of_clouds_; double timeout_sec_; bool debug_mode_; @@ -87,3 +95,5 @@ class CloudCollector }; } // namespace autoware::pointcloud_preprocessor + +#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.ipp" diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.ipp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.ipp new file mode 100644 index 0000000000000..b45ac0bee2a17 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.ipp @@ -0,0 +1,171 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/traits.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +template +CloudCollector::CloudCollector( + std::shared_ptr> && + ros2_parent_node, + std::shared_ptr> & combine_cloud_handler, int num_of_clouds, + double timeout_sec, bool debug_mode) +: ros2_parent_node_(std::move(ros2_parent_node)), + combine_cloud_handler_(combine_cloud_handler), + num_of_clouds_(num_of_clouds), + timeout_sec_(timeout_sec), + debug_mode_(debug_mode) +{ + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + + timer_ = + rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { + std::lock_guard concatenate_lock(concatenate_mutex_); + if (concatenate_finished_) return; + concatenate_callback(); + }); +} + +template +void CloudCollector::set_info(std::shared_ptr collector_info) +{ + collector_info_ = std::move(collector_info); +} + +template +std::shared_ptr CloudCollector::get_info() const +{ + return collector_info_; +} + +template +bool CloudCollector::topic_exists(const std::string & topic_name) +{ + return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end(); +} + +template +bool CloudCollector::process_pointcloud( + const std::string & topic_name, typename MsgTraits::PointCloudMessage::ConstSharedPtr cloud) +{ + std::lock_guard concatenate_lock(concatenate_mutex_); + if (concatenate_finished_) return false; + + // Check if the map already contains an entry for the same topic. This shouldn't happen if the + // parameter 'lidar_timestamp_noise_window' is set correctly. + if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "Topic '" << topic_name + << "' already exists in the collector. Check the timestamp of the pointcloud."); + } + topic_to_cloud_map_[topic_name] = cloud; + if (topic_to_cloud_map_.size() == num_of_clouds_) { + concatenate_callback(); + } + + return true; +} + +template +bool CloudCollector::concatenate_finished() const +{ + return concatenate_finished_; +} + +template +void CloudCollector::concatenate_callback() +{ + if (debug_mode_) { + show_debug_message(); + } + + // All pointclouds are received or the timer has timed out, cancel the timer and concatenate the + // pointclouds in the collector. + timer_->cancel(); + + auto concatenated_cloud_result = concatenate_pointclouds(topic_to_cloud_map_); + + ros2_parent_node_->publish_clouds(std::move(concatenated_cloud_result), collector_info_); + + combine_cloud_handler_->allocate_pointclouds(); + + concatenate_finished_ = true; +} + +template +ConcatenatedCloudResult CloudCollector::concatenate_pointclouds( + std::unordered_map + topic_to_cloud_map) +{ + return combine_cloud_handler_->combine_pointclouds(topic_to_cloud_map); +} + +template +std::unordered_map +CloudCollector::get_topic_to_cloud_map() +{ + return topic_to_cloud_map_; +} + +template +void CloudCollector::show_debug_message() +{ + RCLCPP_WARN(ros2_parent_node_->get_logger(), "start show_debug_message timer with timeout=%f", timeout_sec_); + auto time_until_trigger = timer_->time_until_trigger(); + std::stringstream log_stream; + log_stream << std::fixed << std::setprecision(6); + log_stream << "Collector's concatenate callback time: " + << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; + + if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Advanced strategy:\n Collector's reference time min: " + << advanced_info->timestamp - advanced_info->noise_window + << " to max: " << advanced_info->timestamp + advanced_info->noise_window + << " seconds\n"; + } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp + << " seconds\n"; + } + + log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; + + log_stream << "Pointclouds: ["; + std::string separator = ""; + for (const auto & [topic, cloud] : topic_to_cloud_map_) { + log_stream << separator; + log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; + separator = ", "; + } + + log_stream << "]\n"; + + RCLCPP_WARN(ros2_parent_node_->get_logger(), "end show_debug_message timer with timeout=%f", timeout_sec_); + RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp index c314b24a07921..89cca259ea82c 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp @@ -14,8 +14,6 @@ #pragma once -#include "cloud_collector.hpp" - #include #include @@ -30,6 +28,10 @@ namespace autoware::pointcloud_preprocessor { +// Forward declaration of templated class +template +class CloudCollector; + struct MatchingParams { std::string topic_name; @@ -37,45 +39,55 @@ struct MatchingParams double cloud_arrival_time; }; +template class CollectorMatchingStrategy { public: virtual ~CollectorMatchingStrategy() = default; - [[nodiscard]] virtual std::optional> match_cloud_to_collector( - const std::list> & cloud_collectors, + [[nodiscard]] virtual std::optional>> + match_cloud_to_collector( + const std::list>> & cloud_collectors, const MatchingParams & params) const = 0; virtual void set_collector_info( - std::shared_ptr & collector, const MatchingParams & matching_params) = 0; + std::shared_ptr> & collector, + const MatchingParams & matching_params) = 0; }; -class NaiveMatchingStrategy : public CollectorMatchingStrategy +template +class NaiveMatchingStrategy : public CollectorMatchingStrategy { public: explicit NaiveMatchingStrategy(rclcpp::Node & node); - [[nodiscard]] std::optional> match_cloud_to_collector( - const std::list> & cloud_collectors, + [[nodiscard]] std::optional>> match_cloud_to_collector( + const std::list>> & cloud_collectors, const MatchingParams & params) const override; void set_collector_info( - std::shared_ptr & collector, const MatchingParams & matching_params) override; + std::shared_ptr> & collector, + const MatchingParams & matching_params) override; }; -class AdvancedMatchingStrategy : public CollectorMatchingStrategy +template +class AdvancedMatchingStrategy : public CollectorMatchingStrategy { public: explicit AdvancedMatchingStrategy(rclcpp::Node & node, std::vector input_topics); - [[nodiscard]] std::optional> match_cloud_to_collector( - const std::list> & cloud_collectors, + [[nodiscard]] std::optional>> match_cloud_to_collector( + const std::list>> & cloud_collectors, const MatchingParams & params) const override; void set_collector_info( - std::shared_ptr & collector, const MatchingParams & matching_params) override; + std::shared_ptr> & collector, + const MatchingParams & matching_params) override; private: std::unordered_map topic_to_offset_map_; std::unordered_map topic_to_noise_window_map_; }; -std::shared_ptr parse_matching_strategy(rclcpp::Node & node); +template +std::shared_ptr> parse_matching_strategy(rclcpp::Node & node); } // namespace autoware::pointcloud_preprocessor + +#include "autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.ipp" diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.ipp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.ipp new file mode 100644 index 0000000000000..2ee8c411dd7df --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.ipp @@ -0,0 +1,129 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/traits.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +template +NaiveMatchingStrategy::NaiveMatchingStrategy(rclcpp::Node & node) +{ + RCLCPP_INFO(node.get_logger(), "Utilize naive matching strategy"); +} + +template +std::optional>> +NaiveMatchingStrategy::match_cloud_to_collector( + const std::list>> & cloud_collectors, + const MatchingParams & params) const +{ + std::optional smallest_time_difference = std::nullopt; + std::shared_ptr> closest_collector = nullptr; + + for (const auto & cloud_collector : cloud_collectors) { + if (!cloud_collector->topic_exists(params.topic_name)) { + auto info = cloud_collector->get_info(); + if (auto naive_info = std::dynamic_pointer_cast(info)) { + double time_difference = std::abs(params.cloud_arrival_time - naive_info->timestamp); + if (!smallest_time_difference || time_difference < smallest_time_difference) { + smallest_time_difference = time_difference; + closest_collector = cloud_collector; + } + } + } + } + + if (closest_collector != nullptr) { + return closest_collector; + } + return std::nullopt; +} + +template +void NaiveMatchingStrategy::set_collector_info( + std::shared_ptr> & collector, const MatchingParams & matching_params) +{ + auto info = std::make_shared(matching_params.cloud_arrival_time); + collector->set_info(info); +} + +template +AdvancedMatchingStrategy::AdvancedMatchingStrategy( + rclcpp::Node & node, std::vector input_topics) +{ + auto lidar_timestamp_offsets = + node.declare_parameter>("matching_strategy.lidar_timestamp_offsets"); + auto lidar_timestamp_noise_window = + node.declare_parameter>("matching_strategy.lidar_timestamp_noise_window"); + + if (lidar_timestamp_offsets.size() != input_topics.size()) { + throw std::runtime_error( + "The number of topics does not match the number of timestamp offsets."); + } + if (lidar_timestamp_noise_window.size() != input_topics.size()) { + throw std::runtime_error( + "The number of topics does not match the number of timestamp noise window."); + } + + for (size_t i = 0; i < input_topics.size(); i++) { + topic_to_offset_map_[input_topics[i]] = lidar_timestamp_offsets[i]; + topic_to_noise_window_map_[input_topics[i]] = lidar_timestamp_noise_window[i]; + } + + RCLCPP_INFO(node.get_logger(), "Utilize advanced matching strategy"); +} + +template +std::optional>> +AdvancedMatchingStrategy::match_cloud_to_collector( + const std::list>> & cloud_collectors, + const MatchingParams & params) const +{ + for (const auto & cloud_collector : cloud_collectors) { + auto info = cloud_collector->get_info(); + if (auto advanced_info = std::dynamic_pointer_cast(info)) { + auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + double time = params.cloud_timestamp - topic_to_offset_map_.at(params.topic_name); + if ( + time < reference_timestamp_max + topic_to_noise_window_map_.at(params.topic_name) && + time > reference_timestamp_min - topic_to_noise_window_map_.at(params.topic_name)) { + return cloud_collector; + } + } + } + return std::nullopt; +} + +template +void AdvancedMatchingStrategy::set_collector_info( + std::shared_ptr> & collector, const MatchingParams & matching_params) +{ + auto info = std::make_shared( + matching_params.cloud_timestamp - topic_to_offset_map_.at(matching_params.topic_name), + topic_to_noise_window_map_[matching_params.topic_name]); + collector->set_info(info); +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp index fa8c58aa74c11..7a0c531e7797a 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp @@ -14,6 +14,8 @@ #pragma once +#include "traits.hpp" + #include #include #include @@ -48,19 +50,48 @@ namespace autoware::pointcloud_preprocessor using autoware::point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; +template struct ConcatenatedCloudResult { - sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr}; - std::optional> + typename MsgTraits::PointCloudMessage::UniquePtr concatenate_cloud_ptr{nullptr}; + std::optional> topic_to_transformed_cloud_map; std::unordered_map topic_to_original_stamp_map; }; -class CombineCloudHandler +class CombineCloudHandlerBase { -private: - rclcpp::Node & node_; +public: + CombineCloudHandlerBase( + rclcpp::Node & node, const std::vector & input_topics, std::string output_frame, + bool is_motion_compensated, bool publish_synchronized_pointcloud, + bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only) + : node_(node), + input_topics_(input_topics), + output_frame_(output_frame), + is_motion_compensated_(is_motion_compensated), + publish_synchronized_pointcloud_(publish_synchronized_pointcloud), + keep_input_frame_in_synchronized_pointcloud_(keep_input_frame_in_synchronized_pointcloud), + managed_tf_buffer_(std::make_unique( + &node_, has_static_tf_only)) + { + } + + void process_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg); + + void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input); + + std::deque get_twist_queue(); + + Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + + virtual void allocate_pointclouds() = 0; +protected: + rclcpp::Node & node_; + std::vector input_topics_; std::string output_frame_; bool is_motion_compensated_; bool publish_synchronized_pointcloud_; @@ -68,7 +99,32 @@ class CombineCloudHandler std::unique_ptr managed_tf_buffer_{nullptr}; std::deque twist_queue_; +}; +template +class CombineCloudHandler; + +template <> +class CombineCloudHandler : public CombineCloudHandlerBase +{ +public: + CombineCloudHandler( + rclcpp::Node & node, const std::vector & input_topics, std::string output_frame, + bool is_motion_compensated, bool publish_synchronized_pointcloud, + bool keep_input_frame_in_synchronized_pointcloud, bool has_static_tf_only) + : CombineCloudHandlerBase( + node, input_topics, output_frame, is_motion_compensated, publish_synchronized_pointcloud, + keep_input_frame_in_synchronized_pointcloud, has_static_tf_only) + { + } + + ConcatenatedCloudResult combine_pointclouds( + std::unordered_map & + topic_to_cloud_map); + + void allocate_pointclouds() override {}; + +protected: /// @brief RclcppTimeHash structure defines a custom hash function for the rclcpp::Time type by /// using its nanoseconds representation as the hash value. struct RclcppTimeHash @@ -80,31 +136,15 @@ class CombineCloudHandler }; static void convert_to_xyzirc_cloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud, - sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud); + const typename PointCloud2Traits::PointCloudMessage::ConstSharedPtr & input_cloud, + typename PointCloud2Traits::PointCloudMessage::UniquePtr & xyzirc_cloud); void correct_pointcloud_motion( - const std::shared_ptr & transformed_cloud_ptr, + const std::unique_ptr & transformed_cloud_ptr, const std::vector & pc_stamps, std::unordered_map & transform_memo, - std::shared_ptr transformed_delay_compensated_cloud_ptr); - -public: - CombineCloudHandler( - rclcpp::Node & node, std::string output_frame, bool is_motion_compensated, - bool publish_synchronized_pointcloud, bool keep_input_frame_in_synchronized_pointcloud, - bool has_static_tf_only); - void process_twist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg); - void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg); - - ConcatenatedCloudResult combine_pointclouds( - std::unordered_map & topic_to_cloud_map); - - Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); - - std::deque get_twist_queue(); + std::unique_ptr & + transformed_delay_compensated_cloud_ptr); }; } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp index 654593e317ad9..2e2a443f1b79c 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp @@ -25,6 +25,7 @@ #include "cloud_collector.hpp" #include "collector_matching_strategy.hpp" #include "combine_cloud_handler.hpp" +#include "traits.hpp" #include #include @@ -48,17 +49,29 @@ namespace autoware::pointcloud_preprocessor { -class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node + +template +class PointCloudConcatenateDataSynchronizerComponentTemplated : public rclcpp::Node { public: - explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); - ~PointCloudConcatenateDataSynchronizerComponent() override = default; + using PointCloudMessage = typename MsgTraits::PointCloudMessage; + using PublisherType = typename MsgTraits::PublisherType; + using SubscriberType = typename MsgTraits::SubscriberType; + + explicit PointCloudConcatenateDataSynchronizerComponentTemplated( + const rclcpp::NodeOptions & node_options); + ~PointCloudConcatenateDataSynchronizerComponentTemplated() override = default; + void publish_clouds( - ConcatenatedCloudResult && concatenated_cloud_result, + ConcatenatedCloudResult && concatenated_cloud_result, std::shared_ptr collector_info); + void manage_collector_list(); - std::list> get_cloud_collectors(); - void add_cloud_collector(const std::shared_ptr & collector); + void delete_collector(CloudCollector & cloud_collector); + + std::list>> get_cloud_collectors(); + + void add_cloud_collector(const std::shared_ptr> & collector); private: struct Parameters @@ -88,30 +101,34 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::shared_ptr diagnostic_collector_info_; std::unordered_map diagnostic_topic_to_original_stamp_map_; - std::shared_ptr combine_cloud_handler_; - std::list> cloud_collectors_; - std::unique_ptr collector_matching_strategy_; + std::shared_ptr> combine_cloud_handler_; + std::list>> cloud_collectors_; + std::unique_ptr> collector_matching_strategy_; + std::mutex cloud_collectors_mutex_; // default postfix name for synchronized pointcloud static constexpr const char * default_sync_topic_postfix = "_synchronized"; // subscribers - std::vector::SharedPtr> pointcloud_subs_; + std::vector> pointcloud_subs_; rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Subscription::SharedPtr odom_sub_; // publishers - rclcpp::Publisher::SharedPtr concatenated_cloud_publisher_; - std::unordered_map::SharedPtr> + std::shared_ptr concatenated_cloud_publisher_; + std::unordered_map> topic_to_transformed_cloud_publisher_map_; std::unique_ptr debug_publisher_; std::unique_ptr> stop_watch_ptr_; diagnostic_updater::Updater diagnostic_updater_{this}; + void initialize(); + void cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); + const typename PointCloudMessage::ConstSharedPtr & input_ptr, const std::string & topic_name); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); @@ -121,4 +138,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const std::string & original_topic_name, const std::string & postfix); }; +class PointCloudConcatenateDataSynchronizerComponent +: public PointCloudConcatenateDataSynchronizerComponentTemplated +{ +public: + explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options) + : PointCloudConcatenateDataSynchronizerComponentTemplated(node_options) + { + } + ~PointCloudConcatenateDataSynchronizerComponent() override = default; +}; + } // namespace autoware::pointcloud_preprocessor + +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.ipp" diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.ipp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.ipp new file mode 100644 index 0000000000000..a130b8d844c61 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.ipp @@ -0,0 +1,446 @@ +// 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +//#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" + +#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +template +PointCloudConcatenateDataSynchronizerComponentTemplated:: + PointCloudConcatenateDataSynchronizerComponentTemplated(const rclcpp::NodeOptions & node_options) +: Node("point_cloud_concatenator_component", node_options) +{ + // initialize debug tool + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + + // initialize parameters + params_.debug_mode = declare_parameter("debug_mode"); + params_.has_static_tf_only = declare_parameter("has_static_tf_only"); + params_.rosbag_length = declare_parameter("rosbag_length"); + params_.maximum_queue_size = static_cast(declare_parameter("maximum_queue_size")); + params_.timeout_sec = declare_parameter("timeout_sec"); + params_.is_motion_compensated = declare_parameter("is_motion_compensated"); + params_.publish_synchronized_pointcloud = + declare_parameter("publish_synchronized_pointcloud"); + params_.keep_input_frame_in_synchronized_pointcloud = + declare_parameter("keep_input_frame_in_synchronized_pointcloud"); + params_.publish_previous_but_late_pointcloud = + declare_parameter("publish_previous_but_late_pointcloud"); + params_.synchronized_pointcloud_postfix = + declare_parameter("synchronized_pointcloud_postfix"); + params_.input_twist_topic_type = declare_parameter("input_twist_topic_type"); + params_.input_topics = declare_parameter>("input_topics"); + params_.output_frame = declare_parameter("output_frame"); + + if (params_.input_topics.empty()) { + throw std::runtime_error("Need a 'input_topics' parameter to be set before continuing."); + } + if (params_.input_topics.size() == 1) { + throw std::runtime_error("Only one topic given. Need at least two topics to continue."); + } + + if (params_.output_frame.empty()) { + throw std::runtime_error("Need an 'output_frame' parameter to be set before continuing."); + } + + params_.matching_strategy = declare_parameter("matching_strategy.type"); + + if (params_.matching_strategy == "naive") { + collector_matching_strategy_ = std::make_unique>(*this); + } else if (params_.matching_strategy == "advanced") { + collector_matching_strategy_ = + std::make_unique>(*this, params_.input_topics); + } else { + throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); + } + + // Implementation independant subscribers + if (params_.is_motion_compensated) { + if (params_.input_twist_topic_type == "twist") { + twist_sub_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, + std::bind( + &PointCloudConcatenateDataSynchronizerComponentTemplated::twist_callback, this, + std::placeholders::_1)); + } else if (params_.input_twist_topic_type == "odom") { + odom_sub_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, + std::bind( + &PointCloudConcatenateDataSynchronizerComponentTemplated::odom_callback, this, + std::placeholders::_1)); + } else { + throw std::runtime_error( + "input_twist_topic_type is invalid: " + params_.input_twist_topic_type); + } + } + + + // Combine cloud handler + combine_cloud_handler_ = std::make_shared>( + *this, params_.input_topics, params_.output_frame, params_.is_motion_compensated, + params_.publish_synchronized_pointcloud, params_.keep_input_frame_in_synchronized_pointcloud, + params_.has_static_tf_only); + + // Diagnostic Updater + diagnostic_updater_.setHardwareID("concatenate_data_checker"); + diagnostic_updater_.add( + "concat_status", this, + &PointCloudConcatenateDataSynchronizerComponentTemplated::check_concat_status); + + initialize(); +} + +template +std::string +PointCloudConcatenateDataSynchronizerComponentTemplated::replace_sync_topic_name_postfix( + const std::string & original_topic_name, const std::string & postfix) +{ + std::string replaced_topic_name; + // separate the topic name by '/' and replace the last element with the new postfix + size_t pos = original_topic_name.find_last_of('/'); + if (pos == std::string::npos) { + // not found '/': this is not a namespaced topic + RCLCPP_WARN_STREAM( + get_logger(), + "The topic name is not namespaced. The postfix will be added to the end of the topic name."); + return original_topic_name + postfix; + } + + // replace the last element with the new postfix + replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; + + // if topic name is the same with original topic name, add postfix to the end of the topic name + if (replaced_topic_name == original_topic_name) { + RCLCPP_WARN_STREAM( + get_logger(), "The topic name " + << original_topic_name + << " have the same postfix with synchronized pointcloud. We use " + "the postfix " + "to the end of the topic name."); + replaced_topic_name = original_topic_name + default_sync_topic_postfix; + } + return replaced_topic_name; +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::cloud_callback( + const typename MsgTraits::PointCloudMessage::ConstSharedPtr & input_ptr, + const std::string & topic_name) +{ + stop_watch_ptr_->toc("processing_time", true); + double cloud_arrival_time = this->get_clock()->now().seconds(); + manage_collector_list(); + + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + + if (params_.debug_mode) { + RCLCPP_INFO( + this->get_logger(), " pointcloud %s timestamp: %lf arrive time: %lf seconds, latency: %lf", + topic_name.c_str(), rclcpp::Time(input_ptr->header.stamp).seconds(), + this->get_clock()->now().seconds(), + this->get_clock()->now().seconds() - rclcpp::Time(input_ptr->header.stamp).seconds()); + } + + if (input_ptr->width * input_ptr->height == 0) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + } + + // protect cloud collectors list + std::unique_lock cloud_collectors_lock(cloud_collectors_mutex_); + + // For each callback, check whether there is a exist collector that matches this cloud + std::optional>> cloud_collector = std::nullopt; + MatchingParams matching_params; + matching_params.topic_name = topic_name; + matching_params.cloud_arrival_time = cloud_arrival_time; + matching_params.cloud_timestamp = rclcpp::Time(input_ptr->header.stamp).seconds(); + + if (!cloud_collectors_.empty()) { + cloud_collector = + collector_matching_strategy_->match_cloud_to_collector(cloud_collectors_, matching_params); + } + + bool process_success = false; + if (cloud_collector.has_value()) { + auto collector = cloud_collector.value(); + if (collector) { + cloud_collectors_lock.unlock(); + process_success = cloud_collector.value()->process_pointcloud(topic_name, input_ptr); + } + } + + if (!process_success) { + auto new_cloud_collector = std::make_shared>( + std::dynamic_pointer_cast>( + shared_from_this()), + combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec, params_.debug_mode); + + cloud_collectors_.push_back(new_cloud_collector); + cloud_collectors_lock.unlock(); + + collector_matching_strategy_->set_collector_info(new_cloud_collector, matching_params); + (void)new_cloud_collector->process_pointcloud(topic_name, input_ptr); + } +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) +{ + combine_cloud_handler_->process_twist(input); +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + combine_cloud_handler_->process_odometry(input); +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::publish_clouds( + ConcatenatedCloudResult && concatenated_cloud_result, + std::shared_ptr collector_info) +{ + // should never come to this state. + if (concatenated_cloud_result.concatenate_cloud_ptr == nullptr) { + RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is a nullptr."); + return; + } + + if ( + concatenated_cloud_result.concatenate_cloud_ptr->width * + concatenated_cloud_result.concatenate_cloud_ptr->height == + 0) { + RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is an empty pointcloud."); + is_concatenated_cloud_empty_ = true; + } + + current_concatenate_cloud_timestamp_ = + rclcpp::Time(concatenated_cloud_result.concatenate_cloud_ptr->header.stamp).seconds(); + + if ( + current_concatenate_cloud_timestamp_ < latest_concatenate_cloud_timestamp_ && + !params_.publish_previous_but_late_pointcloud) { + // Publish the cloud if the rosbag replays in loop + if ( + latest_concatenate_cloud_timestamp_ - current_concatenate_cloud_timestamp_ > + params_.rosbag_length) { + publish_pointcloud_ = true; // Force publishing in this case + } else { + drop_previous_but_late_pointcloud_ = true; // Otherwise, drop the late pointcloud + } + } else { + // Publish pointcloud if timestamps are valid or the condition doesn't apply + publish_pointcloud_ = true; + } + + if (publish_pointcloud_) { + latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; + concatenated_cloud_publisher_->publish(std::move(concatenated_cloud_result.concatenate_cloud_ptr)); + + // publish transformed raw pointclouds + if ( + params_.publish_synchronized_pointcloud && + concatenated_cloud_result.topic_to_transformed_cloud_map) { + for (const auto & topic : params_.input_topics) { + // Get a reference to the internal map + if ( + (*concatenated_cloud_result.topic_to_transformed_cloud_map).find(topic) != + (*concatenated_cloud_result.topic_to_transformed_cloud_map).end()) { + auto transformed_cloud_output = std::move((*concatenated_cloud_result.topic_to_transformed_cloud_map).at(topic)); + topic_to_transformed_cloud_publisher_map_[topic]->publish( + std::move(transformed_cloud_output)); + } else { + RCLCPP_WARN( + this->get_logger(), + "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", topic.c_str()); + } + } + } + } + + diagnostic_collector_info_ = collector_info; + + diagnostic_topic_to_original_stamp_map_ = concatenated_cloud_result.topic_to_original_stamp_map; + diagnostic_updater_.force_update(); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + + for (const auto & [topic, stamp] : concatenated_cloud_result.topic_to_original_stamp_map) { + const auto pipeline_latency_ms = (this->get_clock()->now().seconds() - stamp) * 1000; + debug_publisher_->publish( + "debug" + topic + "/pipeline_latency_ms", pipeline_latency_ms); + } + } +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::manage_collector_list() +{ + std::lock_guard cloud_collectors_lock(cloud_collectors_mutex_); + + for (auto it = cloud_collectors_.begin(); it != cloud_collectors_.end();) { + if ((*it)->concatenate_finished()) { + it = cloud_collectors_.erase(it); // Erase and move the iterator to the next element + } else { + ++it; // Move to the next element + } + } +} + +template +std::string PointCloudConcatenateDataSynchronizerComponentTemplated::format_timestamp( + double timestamp) +{ + std::ostringstream oss; + oss << std::fixed << std::setprecision(9) << timestamp; + return oss.str(); +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::check_concat_status( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (publish_pointcloud_ || drop_previous_but_late_pointcloud_) { + stat.add( + "concatenated_cloud_timestamp", format_timestamp(current_concatenate_cloud_timestamp_)); + + if ( + auto naive_info = std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add("first_cloud_arrival_timestamp", format_timestamp(naive_info->timestamp)); + } else if ( + auto advanced_info = + std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add( + "reference_timestamp_min", + format_timestamp(advanced_info->timestamp - advanced_info->noise_window)); + stat.add( + "reference_timestamp_max", + format_timestamp(advanced_info->timestamp + advanced_info->noise_window)); + } + + bool topic_miss = false; + + bool concatenation_success = true; + for (const auto & topic : params_.input_topics) { + bool input_cloud_concatenated = true; + if ( + diagnostic_topic_to_original_stamp_map_.find(topic) != + diagnostic_topic_to_original_stamp_map_.end()) { + stat.add( + topic + "/timestamp", format_timestamp(diagnostic_topic_to_original_stamp_map_[topic])); + } else { + topic_miss = true; + concatenation_success = false; + input_cloud_concatenated = false; + } + stat.add(topic + "/is_concatenated", input_cloud_concatenated); + } + + stat.add("cloud_concatenation_success", concatenation_success); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string message = "Concatenated pointcloud is published and include all topics"; + + if (drop_previous_but_late_pointcloud_) { + if (topic_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = + "Concatenated pointcloud misses some topics and is not published because it arrived " + "too late"; + } else { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is not published as it is too late"; + } + } else { + if (is_concatenated_cloud_empty_) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is empty"; + } else if (topic_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is published but misses some topics"; + } + } + + stat.summary(level, message); + + publish_pointcloud_ = false; + drop_previous_but_late_pointcloud_ = false; + is_concatenated_cloud_empty_ = false; + } else { + const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + const std::string message = + "Concatenate node launch successfully, but waiting for input pointcloud"; + stat.summary(level, message); + } +} + +template +std::list>> +PointCloudConcatenateDataSynchronizerComponentTemplated::get_cloud_collectors() +{ + return cloud_collectors_; +} + +template +void PointCloudConcatenateDataSynchronizerComponentTemplated::add_cloud_collector( + const std::shared_ptr> & collector) +{ + cloud_collectors_.push_back(collector); +} + + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/traits.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/traits.hpp new file mode 100644 index 0000000000000..de35c95605020 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/traits.hpp @@ -0,0 +1,31 @@ +// Copyright 2025 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. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include + +namespace autoware::pointcloud_preprocessor +{ + +struct PointCloud2Traits +{ + using PointCloudMessage = sensor_msgs::msg::PointCloud2; + using PublisherType = rclcpp::Publisher; + using SubscriberType = rclcpp::Subscription; +}; + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp index 4810c5ce84130..db8b440be8126 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/utility/memory.hpp @@ -17,20 +17,42 @@ #include +#include + namespace autoware::pointcloud_preprocessor::utils { +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to + * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ +bool is_data_layout_compatible_with_point_xyzi( + const std::vector & fields); + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZI. That is to * say whether you can memcpy from the PointCloud2 data buffer to a PointXYZI */ bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input); +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is + * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ +bool is_data_layout_compatible_with_point_xyzirc( + const std::vector & fields); + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRC. That is * to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRC */ bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input); +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ +bool is_data_layout_compatible_with_point_xyziradrt( + const std::vector & fields); + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRADRT. That * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */ bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input); +/** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That + * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ +bool is_data_layout_compatible_with_point_xyzircaedt( + const std::vector & fields); + /** \brief Return whether the input PointCloud2 data has the same layout than PointXYZIRCAEDT. That * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRCAEDT */ bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input); diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 525b1f3eb0699..da207751a373f 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -32,6 +32,7 @@ autoware_point_types autoware_universe_utils cgal + cuda_blackboard cv_bridge diagnostic_updater image_transport diff --git a/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json index c3758d05a8349..6b553c4a4e874 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json @@ -9,7 +9,7 @@ "debug_mode": { "type": "boolean", "default": false, - "description": "Flag to enables debug mode to display additional logging information." + "description": "Flag to enable debug mode to display additional logging information." }, "has_static_tf_only": { "type": "boolean", diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp index 63ee23d204788..19a36e1c8394b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp @@ -14,143 +14,5 @@ #include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" -#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" -#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ - -CloudCollector::CloudCollector( - std::shared_ptr && ros2_parent_node, - std::shared_ptr & combine_cloud_handler, int num_of_clouds, - double timeout_sec, bool debug_mode) -: ros2_parent_node_(std::move(ros2_parent_node)), - combine_cloud_handler_(combine_cloud_handler), - num_of_clouds_(num_of_clouds), - timeout_sec_(timeout_sec), - debug_mode_(debug_mode) -{ - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(timeout_sec_)); - - timer_ = - rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { - std::lock_guard concatenate_lock(concatenate_mutex_); - if (concatenate_finished_) return; - concatenate_callback(); - }); -} - -void CloudCollector::set_info(std::shared_ptr collector_info) -{ - collector_info_ = std::move(collector_info); -} - -std::shared_ptr CloudCollector::get_info() const -{ - return collector_info_; -} - -bool CloudCollector::topic_exists(const std::string & topic_name) -{ - return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end(); -} - -bool CloudCollector::process_pointcloud( - const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud) -{ - std::lock_guard concatenate_lock(concatenate_mutex_); - if (concatenate_finished_) return false; - - // Check if the map already contains an entry for the same topic. This shouldn't happen if the - // parameter 'lidar_timestamp_noise_window' is set correctly. - if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) { - RCLCPP_WARN_STREAM_THROTTLE( - ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), - std::chrono::milliseconds(10000).count(), - "Topic '" << topic_name - << "' already exists in the collector. Check the timestamp of the pointcloud."); - } - topic_to_cloud_map_[topic_name] = cloud; - if (topic_to_cloud_map_.size() == num_of_clouds_) { - concatenate_callback(); - } - - return true; -} - -bool CloudCollector::concatenate_finished() const -{ - return concatenate_finished_; -} - -void CloudCollector::concatenate_callback() -{ - if (debug_mode_) { - show_debug_message(); - } - - // All pointclouds are received or the timer has timed out, cancel the timer and concatenate the - // pointclouds in the collector. - timer_->cancel(); - - auto concatenated_cloud_result = concatenate_pointclouds(topic_to_cloud_map_); - - ros2_parent_node_->publish_clouds(std::move(concatenated_cloud_result), collector_info_); - - concatenate_finished_ = true; -} - -ConcatenatedCloudResult CloudCollector::concatenate_pointclouds( - std::unordered_map topic_to_cloud_map) -{ - return combine_cloud_handler_->combine_pointclouds(topic_to_cloud_map); -} - -std::unordered_map -CloudCollector::get_topic_to_cloud_map() -{ - return topic_to_cloud_map_; -} - -void CloudCollector::show_debug_message() -{ - auto time_until_trigger = timer_->time_until_trigger(); - std::stringstream log_stream; - log_stream << std::fixed << std::setprecision(6); - log_stream << "Collector's concatenate callback time: " - << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; - - if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { - log_stream << "Advanced strategy:\n Collector's reference time min: " - << advanced_info->timestamp - advanced_info->noise_window - << " to max: " << advanced_info->timestamp + advanced_info->noise_window - << " seconds\n"; - } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { - log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp - << " seconds\n"; - } - - log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; - - log_stream << "Pointclouds: ["; - std::string separator = ""; - for (const auto & [topic, cloud] : topic_to_cloud_map_) { - log_stream << separator; - log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; - separator = ", "; - } - - log_stream << "]\n"; - - RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); -} - -} // namespace autoware::pointcloud_preprocessor +template class autoware::pointcloud_preprocessor::CloudCollector< + autoware::pointcloud_preprocessor::PointCloud2Traits>; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp index 50058df3d91af..024677fb3a600 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp @@ -12,110 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" - #include -#include - -#include -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ - -NaiveMatchingStrategy::NaiveMatchingStrategy(rclcpp::Node & node) -{ - RCLCPP_INFO(node.get_logger(), "Utilize naive matching strategy"); -} - -std::optional> NaiveMatchingStrategy::match_cloud_to_collector( - const std::list> & cloud_collectors, - const MatchingParams & params) const -{ - std::optional smallest_time_difference = std::nullopt; - std::shared_ptr closest_collector = nullptr; - - for (const auto & cloud_collector : cloud_collectors) { - if (!cloud_collector->topic_exists(params.topic_name)) { - auto info = cloud_collector->get_info(); - if (auto naive_info = std::dynamic_pointer_cast(info)) { - double time_difference = std::abs(params.cloud_arrival_time - naive_info->timestamp); - if (!smallest_time_difference || time_difference < smallest_time_difference) { - smallest_time_difference = time_difference; - closest_collector = cloud_collector; - } - } - } - } - - if (closest_collector != nullptr) { - return closest_collector; - } - return std::nullopt; -} - -void NaiveMatchingStrategy::set_collector_info( - std::shared_ptr & collector, const MatchingParams & matching_params) -{ - auto info = std::make_shared(matching_params.cloud_arrival_time); - collector->set_info(info); -} - -AdvancedMatchingStrategy::AdvancedMatchingStrategy( - rclcpp::Node & node, std::vector input_topics) -{ - auto lidar_timestamp_offsets = - node.declare_parameter>("matching_strategy.lidar_timestamp_offsets"); - auto lidar_timestamp_noise_window = - node.declare_parameter>("matching_strategy.lidar_timestamp_noise_window"); - - if (lidar_timestamp_offsets.size() != input_topics.size()) { - throw std::runtime_error( - "The number of topics does not match the number of timestamp offsets."); - } - if (lidar_timestamp_noise_window.size() != input_topics.size()) { - throw std::runtime_error( - "The number of topics does not match the number of timestamp noise window."); - } - - for (size_t i = 0; i < input_topics.size(); i++) { - topic_to_offset_map_[input_topics[i]] = lidar_timestamp_offsets[i]; - topic_to_noise_window_map_[input_topics[i]] = lidar_timestamp_noise_window[i]; - } - - RCLCPP_INFO(node.get_logger(), "Utilize advanced matching strategy"); -} - -std::optional> AdvancedMatchingStrategy::match_cloud_to_collector( - const std::list> & cloud_collectors, - const MatchingParams & params) const -{ - for (const auto & cloud_collector : cloud_collectors) { - auto info = cloud_collector->get_info(); - if (auto advanced_info = std::dynamic_pointer_cast(info)) { - auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; - auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; - double time = params.cloud_timestamp - topic_to_offset_map_.at(params.topic_name); - if ( - time < reference_timestamp_max + topic_to_noise_window_map_.at(params.topic_name) && - time > reference_timestamp_min - topic_to_noise_window_map_.at(params.topic_name)) { - return cloud_collector; - } - } - } - return std::nullopt; -} - -void AdvancedMatchingStrategy::set_collector_info( - std::shared_ptr & collector, const MatchingParams & matching_params) -{ - auto info = std::make_shared( - matching_params.cloud_timestamp - topic_to_offset_map_.at(matching_params.topic_name), - topic_to_noise_window_map_[matching_params.topic_name]); - collector->set_info(info); -} -} // namespace autoware::pointcloud_preprocessor +template class autoware::pointcloud_preprocessor::NaiveMatchingStrategy< + autoware::pointcloud_preprocessor::PointCloud2Traits>; +template class autoware::pointcloud_preprocessor::AdvancedMatchingStrategy< + autoware::pointcloud_preprocessor::PointCloud2Traits>; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp index d68218314830b..7ae8243042bad 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp @@ -14,6 +14,8 @@ #include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/traits.hpp" + #include #include @@ -30,21 +32,7 @@ namespace autoware::pointcloud_preprocessor { -CombineCloudHandler::CombineCloudHandler( - rclcpp::Node & node, std::string output_frame, bool is_motion_compensated, - bool publish_synchronized_pointcloud, bool keep_input_frame_in_synchronized_pointcloud, - bool has_static_tf_only) -: node_(node), - output_frame_(std::move(output_frame)), - is_motion_compensated_(is_motion_compensated), - publish_synchronized_pointcloud_(publish_synchronized_pointcloud), - keep_input_frame_in_synchronized_pointcloud_(keep_input_frame_in_synchronized_pointcloud), - managed_tf_buffer_( - std::make_unique(&node_, has_static_tf_only)) -{ -} - -void CombineCloudHandler::process_twist( +void CombineCloudHandlerBase::process_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg) { geometry_msgs::msg::TwistStamped msg; @@ -71,7 +59,7 @@ void CombineCloudHandler::process_twist( twist_queue_.push_back(msg); } -void CombineCloudHandler::process_odometry( +void CombineCloudHandlerBase::process_odometry( const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg) { geometry_msgs::msg::TwistStamped msg; @@ -98,14 +86,81 @@ void CombineCloudHandler::process_odometry( twist_queue_.push_back(msg); } -std::deque CombineCloudHandler::get_twist_queue() +std::deque CombineCloudHandlerBase::get_twist_queue() { return twist_queue_; } -void CombineCloudHandler::convert_to_xyzirc_cloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud, - sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud) +Eigen::Matrix4f CombineCloudHandlerBase::compute_transform_to_adjust_for_old_timestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) +{ + // return identity if no twist is available + if (twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp. Leaving point cloud " + "untransformed."); + return Eigen::Matrix4f::Identity(); + } + + auto old_twist_it = std::lower_bound( + std::begin(twist_queue_), std::end(twist_queue_), old_stamp, + [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { + return rclcpp::Time(x.header.stamp) < t; + }); + old_twist_it = old_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : old_twist_it; + + auto new_twist_it = std::lower_bound( + std::begin(twist_queue_), std::end(twist_queue_), new_stamp, + [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { + return rclcpp::Time(x.header.stamp) < t; + }); + new_twist_it = new_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : new_twist_it; + + auto prev_time = old_stamp; + double x = 0.0; + double y = 0.0; + double yaw = 0.0; + tf2::Quaternion baselink_quat{}; + for (auto twist_it = old_twist_it; twist_it != new_twist_it + 1; ++twist_it) { + const double dt = + (twist_it != new_twist_it) + ? (rclcpp::Time((*twist_it).header.stamp) - rclcpp::Time(prev_time)).seconds() + : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); + + if (std::fabs(dt) > 0.1) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), + "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " + "timestamp"); + break; + } + + const double distance = (*twist_it).twist.linear.x * dt; + yaw += (*twist_it).twist.angular.z * dt; + x += distance * std::cos(yaw); + y += distance * std::sin(yaw); + prev_time = (*twist_it).header.stamp; + } + + Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); + + float cos_yaw = std::cos(yaw); + float sin_yaw = std::sin(yaw); + + transformation_matrix(0, 3) = x; + transformation_matrix(1, 3) = y; + transformation_matrix(0, 0) = cos_yaw; + transformation_matrix(0, 1) = -sin_yaw; + transformation_matrix(1, 0) = sin_yaw; + transformation_matrix(1, 1) = cos_yaw; + + return transformation_matrix; +} + +void CombineCloudHandler::convert_to_xyzirc_cloud( + const typename PointCloud2Traits::PointCloudMessage::ConstSharedPtr & input_cloud, + typename PointCloud2Traits::PointCloudMessage::UniquePtr & xyzirc_cloud) { xyzirc_cloud->header = input_cloud->header; @@ -128,14 +183,14 @@ void CombineCloudHandler::convert_to_xyzirc_cloud( return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; }); - sensor_msgs::PointCloud2Iterator it_x(*input_cloud, "x"); - sensor_msgs::PointCloud2Iterator it_y(*input_cloud, "y"); - sensor_msgs::PointCloud2Iterator it_z(*input_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_x(*input_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(*input_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(*input_cloud, "z"); if (has_valid_intensity && has_valid_return_type && has_valid_channel) { - sensor_msgs::PointCloud2Iterator it_i(*input_cloud, "intensity"); - sensor_msgs::PointCloud2Iterator it_r(*input_cloud, "return_type"); - sensor_msgs::PointCloud2Iterator it_c(*input_cloud, "channel"); + sensor_msgs::PointCloud2ConstIterator it_i(*input_cloud, "intensity"); + sensor_msgs::PointCloud2ConstIterator it_r(*input_cloud, "return_type"); + sensor_msgs::PointCloud2ConstIterator it_c(*input_cloud, "channel"); for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { PointXYZIRC point; @@ -158,11 +213,11 @@ void CombineCloudHandler::convert_to_xyzirc_cloud( } } -void CombineCloudHandler::correct_pointcloud_motion( - const std::shared_ptr & transformed_cloud_ptr, +void CombineCloudHandler::correct_pointcloud_motion( + const std::unique_ptr & transformed_cloud_ptr, const std::vector & pc_stamps, std::unordered_map & transform_memo, - std::shared_ptr transformed_delay_compensated_cloud_ptr) + std::unique_ptr & transformed_delay_compensated_cloud_ptr) { Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); rclcpp::Time current_cloud_stamp = rclcpp::Time(transformed_cloud_ptr->header.stamp); @@ -184,10 +239,12 @@ void CombineCloudHandler::correct_pointcloud_motion( adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); } -ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( - std::unordered_map & topic_to_cloud_map) +ConcatenatedCloudResult +CombineCloudHandler::combine_pointclouds( + std::unordered_map & + topic_to_cloud_map) { - ConcatenatedCloudResult concatenate_cloud_result; + ConcatenatedCloudResult concatenate_cloud_result; std::vector pc_stamps; for (const auto & [topic, cloud] : topic_to_cloud_map) { @@ -200,7 +257,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( // Before combining the pointclouds, initialize and reserve space for the concatenated pointcloud concatenate_cloud_result.concatenate_cloud_ptr = - std::make_shared(); + std::make_unique(); // Reserve space based on the total size of the pointcloud data to speed up the concatenation // process @@ -212,40 +269,45 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( for (const auto & [topic, cloud] : topic_to_cloud_map) { // convert to XYZIRC pointcloud if pointcloud is not empty - auto xyzirc_cloud = std::make_shared(); + auto xyzirc_cloud = std::make_unique(); convert_to_xyzirc_cloud(cloud, xyzirc_cloud); - auto transformed_cloud_ptr = std::make_shared(); + auto transformed_cloud_ptr = std::make_unique(); managed_tf_buffer_->transformPointcloud(output_frame_, *xyzirc_cloud, *transformed_cloud_ptr); concatenate_cloud_result.topic_to_original_stamp_map[topic] = rclcpp::Time(cloud->header.stamp).seconds(); // compensate pointcloud - std::shared_ptr transformed_delay_compensated_cloud_ptr; + std::unique_ptr transformed_delay_compensated_cloud_ptr; if (is_motion_compensated_) { - transformed_delay_compensated_cloud_ptr = std::make_shared(); + transformed_delay_compensated_cloud_ptr = std::make_unique(); correct_pointcloud_motion( transformed_cloud_ptr, pc_stamps, transform_memo, transformed_delay_compensated_cloud_ptr); } else { - transformed_delay_compensated_cloud_ptr = transformed_cloud_ptr; + transformed_delay_compensated_cloud_ptr = std::move(transformed_cloud_ptr); } - pcl::concatenatePointCloud( - *concatenate_cloud_result.concatenate_cloud_ptr, *transformed_delay_compensated_cloud_ptr, - *concatenate_cloud_result.concatenate_cloud_ptr); + if ( + transformed_delay_compensated_cloud_ptr->width * + transformed_delay_compensated_cloud_ptr->height > + 0) { + pcl::concatenatePointCloud( + *concatenate_cloud_result.concatenate_cloud_ptr, *transformed_delay_compensated_cloud_ptr, + *concatenate_cloud_result.concatenate_cloud_ptr); + } if (publish_synchronized_pointcloud_) { if (!concatenate_cloud_result.topic_to_transformed_cloud_map) { // Initialize the map if it is not present concatenate_cloud_result.topic_to_transformed_cloud_map = - std::unordered_map(); + std::unordered_map(); } // convert to original sensor frame if necessary bool need_transform_to_sensor_frame = (cloud->header.frame_id != output_frame_); if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { auto transformed_cloud_ptr_in_sensor_frame = - std::make_shared(); + std::make_unique(); managed_tf_buffer_->transformPointcloud( cloud->header.frame_id, *transformed_delay_compensated_cloud_ptr, *transformed_cloud_ptr_in_sensor_frame); @@ -253,12 +315,12 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( transformed_cloud_ptr_in_sensor_frame->header.frame_id = cloud->header.frame_id; (*concatenate_cloud_result.topic_to_transformed_cloud_map)[topic] = - transformed_cloud_ptr_in_sensor_frame; + std::move(transformed_cloud_ptr_in_sensor_frame); } else { transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; (*concatenate_cloud_result.topic_to_transformed_cloud_map)[topic] = - transformed_delay_compensated_cloud_ptr; + std::move(transformed_delay_compensated_cloud_ptr); } } } @@ -267,70 +329,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( return concatenate_cloud_result; } -Eigen::Matrix4f CombineCloudHandler::compute_transform_to_adjust_for_old_timestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) -{ - // return identity if no twist is available - if (twist_queue_.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), - "No twist is available. Please confirm twist topic and timestamp. Leaving point cloud " - "untransformed."); - return Eigen::Matrix4f::Identity(); - } - - auto old_twist_it = std::lower_bound( - std::begin(twist_queue_), std::end(twist_queue_), old_stamp, - [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { - return rclcpp::Time(x.header.stamp) < t; - }); - old_twist_it = old_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : old_twist_it; - - auto new_twist_it = std::lower_bound( - std::begin(twist_queue_), std::end(twist_queue_), new_stamp, - [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { - return rclcpp::Time(x.header.stamp) < t; - }); - new_twist_it = new_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : new_twist_it; - - auto prev_time = old_stamp; - double x = 0.0; - double y = 0.0; - double yaw = 0.0; - for (auto twist_it = old_twist_it; twist_it != new_twist_it + 1; ++twist_it) { - const double dt = - (twist_it != new_twist_it) - ? (rclcpp::Time((*twist_it).header.stamp) - rclcpp::Time(prev_time)).seconds() - : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); - - if (std::fabs(dt) > 0.1) { - RCLCPP_WARN_STREAM_THROTTLE( - node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), - "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " - "timestamp"); - break; - } - - const double distance = (*twist_it).twist.linear.x * dt; - yaw += (*twist_it).twist.angular.z * dt; - x += distance * std::cos(yaw); - y += distance * std::sin(yaw); - prev_time = (*twist_it).header.stamp; - } - - Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); - - float cos_yaw = std::cos(yaw); - float sin_yaw = std::sin(yaw); - - transformation_matrix(0, 3) = x; - transformation_matrix(1, 3) = y; - transformation_matrix(0, 0) = cos_yaw; - transformation_matrix(0, 1) = -sin_yaw; - transformation_matrix(1, 0) = sin_yaw; - transformation_matrix(1, 1) = cos_yaw; - - return transformation_matrix; -} - } // namespace autoware::pointcloud_preprocessor + +template class autoware::pointcloud_preprocessor::CombineCloudHandler< + autoware::pointcloud_preprocessor::PointCloud2Traits>; diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp index e168781c647ed..a76c5293b6c5b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp @@ -33,58 +33,9 @@ namespace autoware::pointcloud_preprocessor { -PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( - const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options) +template <> +void PointCloudConcatenateDataSynchronizerComponentTemplated::initialize() { - // initialize debug tool - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - - // initialize parameters - params_.debug_mode = declare_parameter("debug_mode"); - params_.has_static_tf_only = declare_parameter("has_static_tf_only"); - params_.rosbag_length = declare_parameter("rosbag_length"); - params_.maximum_queue_size = static_cast(declare_parameter("maximum_queue_size")); - params_.timeout_sec = declare_parameter("timeout_sec"); - params_.is_motion_compensated = declare_parameter("is_motion_compensated"); - params_.publish_synchronized_pointcloud = - declare_parameter("publish_synchronized_pointcloud"); - params_.keep_input_frame_in_synchronized_pointcloud = - declare_parameter("keep_input_frame_in_synchronized_pointcloud"); - params_.publish_previous_but_late_pointcloud = - declare_parameter("publish_previous_but_late_pointcloud"); - params_.synchronized_pointcloud_postfix = - declare_parameter("synchronized_pointcloud_postfix"); - params_.input_twist_topic_type = declare_parameter("input_twist_topic_type"); - params_.input_topics = declare_parameter>("input_topics"); - params_.output_frame = declare_parameter("output_frame"); - - if (params_.input_topics.empty()) { - throw std::runtime_error("Need a 'input_topics' parameter to be set before continuing."); - } - if (params_.input_topics.size() == 1) { - throw std::runtime_error("Only one topic given. Need at least two topics to continue."); - } - - if (params_.output_frame.empty()) { - throw std::runtime_error("Need an 'output_frame' parameter to be set before continuing."); - } - - params_.matching_strategy = declare_parameter("matching_strategy.type"); - if (params_.matching_strategy == "naive") { - collector_matching_strategy_ = std::make_unique(*this); - } else if (params_.matching_strategy == "advanced") { - collector_matching_strategy_ = - std::make_unique(*this, params_.input_topics); - } else { - throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); - } - // Publishers concatenated_cloud_publisher_ = this->create_publisher( "output", rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); @@ -101,359 +52,30 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } // Subscribers - if (params_.is_motion_compensated) { - if (params_.input_twist_topic_type == "twist") { - twist_sub_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, - std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, - std::placeholders::_1)); - } else if (params_.input_twist_topic_type == "odom") { - odom_sub_ = this->create_subscription( - "~/input/odom", rclcpp::QoS{100}, - std::bind( - &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, - std::placeholders::_1)); - } else { - throw std::runtime_error( - "input_twist_topic_type is invalid: " + params_.input_twist_topic_type); - } - } - for (const std::string & topic : params_.input_topics) { - std::function callback = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, - topic); + auto callback = [&](const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + this->cloud_callback(msg, topic); + }; auto pointcloud_sub = this->create_subscription( topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size), callback); pointcloud_subs_.push_back(pointcloud_sub); } + RCLCPP_DEBUG_STREAM( get_logger(), "Subscribing to " << params_.input_topics.size() << " user given topics as inputs:"); for (const auto & input_topic : params_.input_topics) { RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); } - - // Combine cloud handler - combine_cloud_handler_ = std::make_shared( - *this, params_.output_frame, params_.is_motion_compensated, - params_.publish_synchronized_pointcloud, params_.keep_input_frame_in_synchronized_pointcloud, - params_.has_static_tf_only); - - // Diagnostic Updater - diagnostic_updater_.setHardwareID("concatenate_data_checker"); - diagnostic_updater_.add( - "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::check_concat_status); -} - -std::string PointCloudConcatenateDataSynchronizerComponent::replace_sync_topic_name_postfix( - const std::string & original_topic_name, const std::string & postfix) -{ - std::string replaced_topic_name; - // separate the topic name by '/' and replace the last element with the new postfix - size_t pos = original_topic_name.find_last_of('/'); - if (pos == std::string::npos) { - // not found '/': this is not a namespaced topic - RCLCPP_WARN_STREAM( - get_logger(), - "The topic name is not namespaced. The postfix will be added to the end of the topic name."); - return original_topic_name + postfix; - } - - // replace the last element with the new postfix - replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; - - // if topic name is the same with original topic name, add postfix to the end of the topic name - if (replaced_topic_name == original_topic_name) { - RCLCPP_WARN_STREAM( - get_logger(), "The topic name " - << original_topic_name - << " have the same postfix with synchronized pointcloud. We use " - "the postfix " - "to the end of the topic name."); - replaced_topic_name = original_topic_name + default_sync_topic_postfix; - } - return replaced_topic_name; -} - -void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) -{ - stop_watch_ptr_->toc("processing_time", true); - double cloud_arrival_time = this->get_clock()->now().seconds(); - manage_collector_list(); - - if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); - - if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), - "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); - } - - return; - } - - if (params_.debug_mode) { - RCLCPP_INFO( - this->get_logger(), " pointcloud %s timestamp: %lf arrive time: %lf seconds, latency: %lf", - topic_name.c_str(), rclcpp::Time(input_ptr->header.stamp).seconds(), cloud_arrival_time, - cloud_arrival_time - rclcpp::Time(input_ptr->header.stamp).seconds()); - } - - if (input_ptr->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } - - // protect cloud collectors list - std::unique_lock cloud_collectors_lock(cloud_collectors_mutex_); - - // For each callback, check whether there is a exist collector that matches this cloud - std::optional> cloud_collector = std::nullopt; - MatchingParams matching_params; - matching_params.topic_name = topic_name; - matching_params.cloud_arrival_time = cloud_arrival_time; - matching_params.cloud_timestamp = rclcpp::Time(input_ptr->header.stamp).seconds(); - - if (!cloud_collectors_.empty()) { - cloud_collector = - collector_matching_strategy_->match_cloud_to_collector(cloud_collectors_, matching_params); - } - - bool process_success = false; - if (cloud_collector.has_value()) { - auto collector = cloud_collector.value(); - if (collector) { - cloud_collectors_lock.unlock(); - process_success = cloud_collector.value()->process_pointcloud(topic_name, input_ptr); - } - } - - if (!process_success) { - auto new_cloud_collector = std::make_shared( - std::dynamic_pointer_cast(shared_from_this()), - combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec, params_.debug_mode); - - cloud_collectors_.push_back(new_cloud_collector); - cloud_collectors_lock.unlock(); - - collector_matching_strategy_->set_collector_info(new_cloud_collector, matching_params); - (void)new_cloud_collector->process_pointcloud(topic_name, input_ptr); - } -} - -void PointCloudConcatenateDataSynchronizerComponent::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) -{ - combine_cloud_handler_->process_twist(input); -} - -void PointCloudConcatenateDataSynchronizerComponent::odom_callback( - const nav_msgs::msg::Odometry::ConstSharedPtr input) -{ - combine_cloud_handler_->process_odometry(input); -} - -void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( - ConcatenatedCloudResult && concatenated_cloud_result, - std::shared_ptr collector_info) -{ - // should never come to this state. - if (concatenated_cloud_result.concatenate_cloud_ptr == nullptr) { - RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is a nullptr."); - return; - } - - if (concatenated_cloud_result.concatenate_cloud_ptr->data.empty()) { - RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is an empty pointcloud."); - is_concatenated_cloud_empty_ = true; - } - - current_concatenate_cloud_timestamp_ = - rclcpp::Time(concatenated_cloud_result.concatenate_cloud_ptr->header.stamp).seconds(); - - if ( - current_concatenate_cloud_timestamp_ < latest_concatenate_cloud_timestamp_ && - !params_.publish_previous_but_late_pointcloud) { - // Publish the cloud if the rosbag replays in loop - if ( - latest_concatenate_cloud_timestamp_ - current_concatenate_cloud_timestamp_ > - params_.rosbag_length) { - publish_pointcloud_ = true; // Force publishing in this case - } else { - drop_previous_but_late_pointcloud_ = true; // Otherwise, drop the late pointcloud - } - } else { - // Publish pointcloud if timestamps are valid or the condition doesn't apply - publish_pointcloud_ = true; - } - - if (publish_pointcloud_) { - latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; - auto concatenate_pointcloud_output = std::make_unique( - std::move(*concatenated_cloud_result.concatenate_cloud_ptr)); - concatenated_cloud_publisher_->publish(std::move(concatenate_pointcloud_output)); - - // publish transformed raw pointclouds - if ( - params_.publish_synchronized_pointcloud && - concatenated_cloud_result.topic_to_transformed_cloud_map) { - for (const auto & topic : params_.input_topics) { - // Get a reference to the internal map - if ( - (*concatenated_cloud_result.topic_to_transformed_cloud_map).find(topic) != - (*concatenated_cloud_result.topic_to_transformed_cloud_map).end()) { - auto transformed_cloud_output = std::make_unique( - *(*concatenated_cloud_result.topic_to_transformed_cloud_map).at(topic)); - topic_to_transformed_cloud_publisher_map_[topic]->publish( - std::move(transformed_cloud_output)); - } else { - RCLCPP_WARN( - this->get_logger(), - "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", topic.c_str()); - } - } - } - } - - diagnostic_collector_info_ = collector_info; - - diagnostic_topic_to_original_stamp_map_ = concatenated_cloud_result.topic_to_original_stamp_map; - diagnostic_updater_.force_update(); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - - for (const auto & [topic, stamp] : concatenated_cloud_result.topic_to_original_stamp_map) { - const auto pipeline_latency_ms = (this->get_clock()->now().seconds() - stamp) * 1000; - debug_publisher_->publish( - "debug" + topic + "/pipeline_latency_ms", pipeline_latency_ms); - } - } -} - -void PointCloudConcatenateDataSynchronizerComponent::manage_collector_list() -{ - std::lock_guard cloud_collectors_lock(cloud_collectors_mutex_); - - for (auto it = cloud_collectors_.begin(); it != cloud_collectors_.end();) { - if ((*it)->concatenate_finished()) { - it = cloud_collectors_.erase(it); // Erase and move the iterator to the next element - } else { - ++it; // Move to the next element - } - } -} - -std::string PointCloudConcatenateDataSynchronizerComponent::format_timestamp(double timestamp) -{ - std::ostringstream oss; - oss << std::fixed << std::setprecision(9) << timestamp; - return oss.str(); -} - -void PointCloudConcatenateDataSynchronizerComponent::check_concat_status( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - if (publish_pointcloud_ || drop_previous_but_late_pointcloud_) { - stat.add( - "concatenated_cloud_timestamp", format_timestamp(current_concatenate_cloud_timestamp_)); - - if ( - auto naive_info = std::dynamic_pointer_cast(diagnostic_collector_info_)) { - stat.add("first_cloud_arrival_timestamp", format_timestamp(naive_info->timestamp)); - } else if ( - auto advanced_info = - std::dynamic_pointer_cast(diagnostic_collector_info_)) { - stat.add( - "reference_timestamp_min", - format_timestamp(advanced_info->timestamp - advanced_info->noise_window)); - stat.add( - "reference_timestamp_max", - format_timestamp(advanced_info->timestamp + advanced_info->noise_window)); - } - - bool topic_miss = false; - - bool concatenation_success = true; - for (const auto & topic : params_.input_topics) { - bool input_cloud_concatenated = true; - if ( - diagnostic_topic_to_original_stamp_map_.find(topic) != - diagnostic_topic_to_original_stamp_map_.end()) { - stat.add( - topic + "/timestamp", format_timestamp(diagnostic_topic_to_original_stamp_map_[topic])); - } else { - topic_miss = true; - concatenation_success = false; - input_cloud_concatenated = false; - } - stat.add(topic + "/is_concatenated", input_cloud_concatenated); - } - - stat.add("cloud_concatenation_success", concatenation_success); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string message = "Concatenated pointcloud is published and include all topics"; - - if (drop_previous_but_late_pointcloud_) { - if (topic_miss) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = - "Concatenated pointcloud misses some topics and is not published because it arrived " - "too late"; - } else { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = "Concatenated pointcloud is not published as it is too late"; - } - } else { - if (is_concatenated_cloud_empty_) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = "Concatenated pointcloud is empty"; - } else if (topic_miss) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - message = "Concatenated pointcloud is published but misses some topics"; - } - } - - stat.summary(level, message); - - publish_pointcloud_ = false; - drop_previous_but_late_pointcloud_ = false; - is_concatenated_cloud_empty_ = false; - } else { - const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - const std::string message = - "Concatenate node launch successfully, but waiting for input pointcloud"; - stat.summary(level, message); - } -} - -std::list> -PointCloudConcatenateDataSynchronizerComponent::get_cloud_collectors() -{ - return cloud_collectors_; -} - -void PointCloudConcatenateDataSynchronizerComponent::add_cloud_collector( - const std::shared_ptr & collector) -{ - cloud_collectors_.push_back(collector); } } // namespace autoware::pointcloud_preprocessor +template class autoware::pointcloud_preprocessor:: + PointCloudConcatenateDataSynchronizerComponentTemplated< + autoware::pointcloud_preprocessor::PointCloud2Traits>; + #include RCLCPP_COMPONENTS_REGISTER_NODE( autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp index c1fd55943d13a..f5085ad295200 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp @@ -16,32 +16,35 @@ #include +#include + namespace autoware::pointcloud_preprocessor::utils { -bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) +bool is_data_layout_compatible_with_point_xyzi( + const std::vector & fields) { using PointIndex = autoware::point_types::PointXYZIIndex; using autoware::point_types::PointXYZI; - if (input.fields.size() < 4) { + if (fields.size() < 4) { return false; } bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + const auto & field_x = fields.at(static_cast(PointIndex::X)); same_layout &= field_x.name == "x"; same_layout &= field_x.offset == offsetof(PointXYZI, x); same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + const auto & field_y = fields.at(static_cast(PointIndex::Y)); same_layout &= field_y.name == "y"; same_layout &= field_y.offset == offsetof(PointXYZI, y); same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + const auto & field_z = fields.at(static_cast(PointIndex::Z)); same_layout &= field_z.name == "z"; same_layout &= field_z.offset == offsetof(PointXYZI, z); same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + const auto & field_intensity = fields.at(static_cast(PointIndex::Intensity)); same_layout &= field_intensity.name == "intensity"; same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity); same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; @@ -49,40 +52,46 @@ bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointClou return same_layout; } -bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input) +bool is_data_layout_compatible_with_point_xyzi(const sensor_msgs::msg::PointCloud2 & input) +{ + return is_data_layout_compatible_with_point_xyzi(input.fields); +} + +bool is_data_layout_compatible_with_point_xyzirc( + const std::vector & fields) { using PointIndex = autoware::point_types::PointXYZIRCIndex; using autoware::point_types::PointXYZIRC; - if (input.fields.size() < 6) { + if (fields.size() < 6) { return false; } bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + const auto & field_x = fields.at(static_cast(PointIndex::X)); same_layout &= field_x.name == "x"; same_layout &= field_x.offset == offsetof(PointXYZIRC, x); same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + const auto & field_y = fields.at(static_cast(PointIndex::Y)); same_layout &= field_y.name == "y"; same_layout &= field_y.offset == offsetof(PointXYZIRC, y); same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + const auto & field_z = fields.at(static_cast(PointIndex::Z)); same_layout &= field_z.name == "z"; same_layout &= field_z.offset == offsetof(PointXYZIRC, z); same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + const auto & field_intensity = fields.at(static_cast(PointIndex::Intensity)); same_layout &= field_intensity.name == "intensity"; same_layout &= field_intensity.offset == offsetof(PointXYZIRC, intensity); same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; same_layout &= field_intensity.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + const auto & field_return_type = fields.at(static_cast(PointIndex::ReturnType)); same_layout &= field_return_type.name == "return_type"; same_layout &= field_return_type.offset == offsetof(PointXYZIRC, return_type); same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; same_layout &= field_return_type.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + const auto & field_ring = fields.at(static_cast(PointIndex::Channel)); same_layout &= field_ring.name == "channel"; same_layout &= field_ring.offset == offsetof(PointXYZIRC, channel); same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; @@ -91,55 +100,61 @@ bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCl return same_layout; } -bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input) +bool is_data_layout_compatible_with_point_xyzirc(const sensor_msgs::msg::PointCloud2 & input) +{ + return is_data_layout_compatible_with_point_xyzirc(input.fields); +} + +bool is_data_layout_compatible_with_point_xyziradrt( + const std::vector & fields) { using PointIndex = autoware::point_types::PointXYZIRADRTIndex; using autoware::point_types::PointXYZIRADRT; - if (input.fields.size() < 9) { + if (fields.size() < 9) { return false; } bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + const auto & field_x = fields.at(static_cast(PointIndex::X)); same_layout &= field_x.name == "x"; same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x); same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + const auto & field_y = fields.at(static_cast(PointIndex::Y)); same_layout &= field_y.name == "y"; same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y); same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + const auto & field_z = fields.at(static_cast(PointIndex::Z)); same_layout &= field_z.name == "z"; same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z); same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + const auto & field_intensity = fields.at(static_cast(PointIndex::Intensity)); same_layout &= field_intensity.name == "intensity"; same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity); same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_intensity.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Ring)); + const auto & field_ring = fields.at(static_cast(PointIndex::Ring)); same_layout &= field_ring.name == "ring"; same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring); same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; same_layout &= field_ring.count == 1; - const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + const auto & field_azimuth = fields.at(static_cast(PointIndex::Azimuth)); same_layout &= field_azimuth.name == "azimuth"; same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth); same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_azimuth.count == 1; - const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + const auto & field_distance = fields.at(static_cast(PointIndex::Distance)); same_layout &= field_distance.name == "distance"; same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance); same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_distance.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + const auto & field_return_type = fields.at(static_cast(PointIndex::ReturnType)); same_layout &= field_return_type.name == "return_type"; same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type); same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; same_layout &= field_return_type.count == 1; - const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + const auto & field_time_stamp = fields.at(static_cast(PointIndex::TimeStamp)); same_layout &= field_time_stamp.name == "time_stamp"; same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp); same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64; @@ -147,60 +162,66 @@ bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::Poin return same_layout; } -bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input) +bool is_data_layout_compatible_with_point_xyziradrt(const sensor_msgs::msg::PointCloud2 & input) +{ + return is_data_layout_compatible_with_point_xyziradrt(input.fields); +} + +bool is_data_layout_compatible_with_point_xyzircaedt( + const std::vector & fields) { using PointIndex = autoware::point_types::PointXYZIRCAEDTIndex; using autoware::point_types::PointXYZIRCAEDT; - if (input.fields.size() != 10) { + if (fields.size() != 10) { return false; } bool same_layout = true; - const auto & field_x = input.fields.at(static_cast(PointIndex::X)); + const auto & field_x = fields.at(static_cast(PointIndex::X)); same_layout &= field_x.name == "x"; same_layout &= field_x.offset == offsetof(PointXYZIRCAEDT, x); same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_x.count == 1; - const auto & field_y = input.fields.at(static_cast(PointIndex::Y)); + const auto & field_y = fields.at(static_cast(PointIndex::Y)); same_layout &= field_y.name == "y"; same_layout &= field_y.offset == offsetof(PointXYZIRCAEDT, y); same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_y.count == 1; - const auto & field_z = input.fields.at(static_cast(PointIndex::Z)); + const auto & field_z = fields.at(static_cast(PointIndex::Z)); same_layout &= field_z.name == "z"; same_layout &= field_z.offset == offsetof(PointXYZIRCAEDT, z); same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_z.count == 1; - const auto & field_intensity = input.fields.at(static_cast(PointIndex::Intensity)); + const auto & field_intensity = fields.at(static_cast(PointIndex::Intensity)); same_layout &= field_intensity.name == "intensity"; same_layout &= field_intensity.offset == offsetof(PointXYZIRCAEDT, intensity); same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8; same_layout &= field_intensity.count == 1; - const auto & field_return_type = input.fields.at(static_cast(PointIndex::ReturnType)); + const auto & field_return_type = fields.at(static_cast(PointIndex::ReturnType)); same_layout &= field_return_type.name == "return_type"; same_layout &= field_return_type.offset == offsetof(PointXYZIRCAEDT, return_type); same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8; same_layout &= field_return_type.count == 1; - const auto & field_ring = input.fields.at(static_cast(PointIndex::Channel)); + const auto & field_ring = fields.at(static_cast(PointIndex::Channel)); same_layout &= field_ring.name == "channel"; same_layout &= field_ring.offset == offsetof(PointXYZIRCAEDT, channel); same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16; same_layout &= field_ring.count == 1; - const auto & field_azimuth = input.fields.at(static_cast(PointIndex::Azimuth)); + const auto & field_azimuth = fields.at(static_cast(PointIndex::Azimuth)); same_layout &= field_azimuth.name == "azimuth"; same_layout &= field_azimuth.offset == offsetof(PointXYZIRCAEDT, azimuth); same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_azimuth.count == 1; - const auto & field_elevation = input.fields.at(static_cast(PointIndex::Elevation)); + const auto & field_elevation = fields.at(static_cast(PointIndex::Elevation)); same_layout &= field_elevation.name == "elevation"; same_layout &= field_elevation.offset == offsetof(PointXYZIRCAEDT, elevation); same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_elevation.count == 1; - const auto & field_distance = input.fields.at(static_cast(PointIndex::Distance)); + const auto & field_distance = fields.at(static_cast(PointIndex::Distance)); same_layout &= field_distance.name == "distance"; same_layout &= field_distance.offset == offsetof(PointXYZIRCAEDT, distance); same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32; same_layout &= field_distance.count == 1; - const auto & field_time_stamp = input.fields.at(static_cast(PointIndex::TimeStamp)); + const auto & field_time_stamp = fields.at(static_cast(PointIndex::TimeStamp)); same_layout &= field_time_stamp.name == "time_stamp"; same_layout &= field_time_stamp.offset == offsetof(PointXYZIRCAEDT, time_stamp); same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32; @@ -208,4 +229,9 @@ bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::Poi return same_layout; } +bool is_data_layout_compatible_with_point_xyzircaedt(const sensor_msgs::msg::PointCloud2 & input) +{ + return is_data_layout_compatible_with_point_xyzircaedt(input.fields); +} + } // namespace autoware::pointcloud_preprocessor::utils diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp index bc3c7e9538c84..5d26a474f45e7 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp @@ -36,11 +36,17 @@ #include #include +using autoware::pointcloud_preprocessor::CloudCollector; +using autoware::pointcloud_preprocessor::CombineCloudHandler; +using autoware::pointcloud_preprocessor::PointCloud2Traits; +using autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent; + class ConcatenateCloudTest : public ::testing::Test { protected: void SetUp() override { + std::vector input_topics{"lidar_top", "lidar_left", "lidar_right"}; rclcpp::NodeOptions node_options; // Instead of "input_topics", other parameters are not used. // They just helps to setup the concatenate node @@ -56,22 +62,19 @@ class ConcatenateCloudTest : public ::testing::Test {"publish_previous_but_late_pointcloud", false}, {"synchronized_pointcloud_postfix", "pointcloud"}, {"input_twist_topic_type", "twist"}, - {"input_topics", std::vector{"lidar_top", "lidar_left", "lidar_right"}}, + {"input_topics", input_topics}, {"output_frame", "base_link"}, {"matching_strategy.type", "advanced"}, {"matching_strategy.lidar_timestamp_offsets", std::vector{0.0, 0.04, 0.08}}, {"matching_strategy.lidar_timestamp_noise_window", std::vector{0.01, 0.01, 0.01}}}); - concatenate_node_ = std::make_shared< - autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( - node_options); - combine_cloud_handler_ = - std::make_shared( - *concatenate_node_, "base_link", true, true, true, false); + concatenate_node_ = + std::make_shared(node_options); + combine_cloud_handler_ = std::make_shared>( + *concatenate_node_, input_topics, "base_link", true, true, true, false); - collector_ = std::make_shared( - std::dynamic_pointer_cast< - autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( + collector_ = std::make_shared>( + std::dynamic_pointer_cast( concatenate_node_->shared_from_this()), combine_cloud_handler_, number_of_pointcloud, timeout_sec, collector_debug_mode); @@ -169,10 +172,9 @@ class ConcatenateCloudTest : public ::testing::Test generate_transform_msg("base_link", "lidar_right", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453); } - std::shared_ptr - concatenate_node_; - std::shared_ptr combine_cloud_handler_; - std::shared_ptr collector_; + std::shared_ptr concatenate_node_; + std::shared_ptr> combine_cloud_handler_; + std::shared_ptr> collector_; std::shared_ptr tf_broadcaster_; static constexpr int32_t timestamp_seconds{10}; @@ -344,7 +346,7 @@ TEST_F(ConcatenateCloudTest, TestConcatenateClouds) sensor_msgs::msg::PointCloud2::SharedPtr right_pointcloud_ptr = std::make_shared(right_pointcloud); - std::unordered_map topic_to_cloud_map; + std::unordered_map topic_to_cloud_map; topic_to_cloud_map["lidar_top"] = top_pointcloud_ptr; topic_to_cloud_map["lidar_left"] = left_pointcloud_ptr; topic_to_cloud_map["lidar_right"] = right_pointcloud_ptr;