From 4f7b536fb78a0d3d3ed932ff418e7ec91082d499 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 22 Nov 2024 13:36:38 +0900 Subject: [PATCH] feat: implemented a cuda accelerated pointcloud concatenation and integrated it with the cuda blackboard Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 18 + .../concatenate_and_time_sync_node.param.yaml | 1 + .../concatenate_data/cloud_collector.hpp | 19 +- .../combine_cloud_handler.hpp | 104 ++++- .../combine_cloud_handler_kernel.hpp | 61 +++ .../concatenate_and_time_sync_node.hpp | 71 ++- .../utility/memory.hpp | 22 + .../package.xml | 1 + .../src/concatenate_data/cloud_collector.cpp | 96 +++- .../combine_cloud_handler.cpp | 410 +++++++++++++++--- .../combine_cloud_handler_kernel.cu | 57 +++ .../concatenate_and_time_sync_node.cpp | 355 ++++++++++++--- .../src/utility/memory.cpp | 98 +++-- .../test/test_concatenate_node_unit.cpp | 28 +- 14 files changed, 1122 insertions(+), 219 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp create mode 100644 sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler_kernel.cu diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index b4c49863fdba6..14dd278fc760a 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -13,11 +13,19 @@ find_package(Sophus REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) find_package(CGAL REQUIRED COMPONENTS Core) +find_package(CUDA) + +if(${CUDA_FOUND}) + add_definitions(-DUSE_CUDA) +else() + message("CUDA wat not found. Disabling CUDA-dependent functionalities") +endif() include_directories( include SYSTEM ${Boost_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${Sophus_INCLUDE_DIRS} @@ -63,6 +71,14 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter sensor_msgs ) +cuda_add_library(pointcloud_preprocessor_filter_cuda SHARED + src/concatenate_data/combine_cloud_handler_kernel.cu +) + +target_include_directories(pointcloud_preprocessor_filter_cuda PUBLIC + ${cuda_blackboard_INCLUDE_DIRS} +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/concatenate_data/concatenate_and_time_sync_node.cpp src/concatenate_data/combine_cloud_handler.cpp @@ -93,8 +109,10 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base + pointcloud_preprocessor_filter_cuda faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} + ${CUDA_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} ${PCL_LIBRARIES} diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml index c5bd139e1e962..cbf3e0d64cd1c 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + use_cuda: false debug_mode: false has_static_tf_only: false rosbag_replay: false 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 b90fd4a526632..855a181029ac7 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 @@ -25,33 +25,36 @@ namespace autoware::pointcloud_preprocessor { class PointCloudConcatenateDataSynchronizerComponent; + +template class CombineCloudHandler; +template class CloudCollector { public: CloudCollector( std::shared_ptr && ros2_parent_node, - std::shared_ptr & combine_cloud_handler, int num_of_clouds, - double timeout_sec, bool debug_mode); + std::shared_ptr> & combine_cloud_handler, + int num_of_clouds, double timeout_sec, bool debug_mode); void set_reference_timestamp(double timestamp, double noise_window); std::tuple get_reference_timestamp_boundary(); void process_pointcloud( - const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud); + const std::string & topic_name, typename 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(); private: std::shared_ptr ros2_parent_node_; - std::shared_ptr combine_cloud_handler_; + 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_; double reference_timestamp_min_{0.0}; 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 41a643fa81e74..856359f860038 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 @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -43,12 +44,20 @@ #include #include +#ifdef USE_CUDA +#include +#endif + namespace autoware::pointcloud_preprocessor { using autoware::point_types::PointXYZIRC; using point_cloud_msg_wrapper::PointCloud2Modifier; -struct ConcatenatedCloudResult +template +struct ConcatenatedCloudResult; + +template <> +struct ConcatenatedCloudResult { sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr}; std::optional> @@ -56,9 +65,20 @@ struct ConcatenatedCloudResult std::unordered_map topic_to_original_stamp_map; }; -class CombineCloudHandler +#ifdef USE_CUDA +template <> +struct ConcatenatedCloudResult +{ + cuda_blackboard::CudaPointCloud2::UniquePtr concatenate_cloud_ptr{nullptr}; + std::optional> + topic_to_transformed_cloud_map; + std::unordered_map topic_to_original_stamp_map; +}; +#endif + +class CombineCloudHandlerBase { -private: +protected: rclcpp::Node & node_; std::vector input_topics_; @@ -80,9 +100,41 @@ class CombineCloudHandler } }; +public: + CombineCloudHandlerBase( + rclcpp::Node & node, 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); + + virtual ~CombineCloudHandlerBase() = default; + + void process_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg); + + void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input); + + Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + + std::deque get_twist_queue(); +}; + +template +class CombineCloudHandler : public CombineCloudHandlerBase +{ +public: + ConcatenatedCloudResult combine_pointclouds( + std::unordered_map & + topic_to_cloud_map); +}; + +template <> +class CombineCloudHandler : public CombineCloudHandlerBase +{ +protected: static void convert_to_xyzirc_cloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud, - sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud); + const typename sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud, + typename sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud); void correct_pointcloud_motion( const std::shared_ptr & transformed_cloud_ptr, @@ -95,16 +147,44 @@ class CombineCloudHandler rclcpp::Node & node, 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); - void process_twist(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & input); - void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & input); - ConcatenatedCloudResult combine_pointclouds( - std::unordered_map & topic_to_cloud_map); + 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); +#ifdef USE_CUDA - std::deque get_twist_queue(); +template <> +class CombineCloudHandler : public CombineCloudHandlerBase +{ +protected: + struct CudaConcatStruct + { + cudaStream_t stream; + std::unique_ptr cloud_ptr; + std::size_t max_pointcloud_size_{0}; + }; + + std::unordered_map cuda_concat_struct_map_; + std::unique_ptr concatenated_cloud_ptr; + std::size_t max_concat_pointcloud_size_{0}; + std::mutex mutex_; + +public: + CombineCloudHandler( + rclcpp::Node & node, 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); + + void allocate_pointclouds(); + +public: + ConcatenatedCloudResult combine_pointclouds( + std::unordered_map & + topic_to_cloud_map); }; +#endif + } // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp new file mode 100644 index 0000000000000..c9a284123315c --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp @@ -0,0 +1,61 @@ +// 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. + +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_ + +#ifdef USE_CUDA +#include +#endif + +#include + +namespace autoware::pointcloud_preprocessor +{ + +struct TransformStruct +{ + float translation_x; + float translation_y; + float translation_z; + float m11; + float m12; + float m13; + float m21; + float m22; + float m23; + float m31; + float m32; + float m33; +}; + +struct PointTypeStruct +{ + float x; + float y; + float z; + std::uint8_t intensity; + std::uint8_t return_type; + std::uint16_t channel; +}; + +#ifdef USE_CUDA +void transform_launch( + const PointTypeStruct * input_points, int num_points, TransformStruct transform, + PointTypeStruct * output_points, cudaStream_t & stream); +#endif + +} // namespace autoware::pointcloud_preprocessor + +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__COMBINE_CLOUD_HANDLER_KERNEL_HPP_ 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 3e6febbdbe844..ca56ca5a0678d 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 @@ -45,6 +45,12 @@ #include #include +#ifdef USE_CUDA +#include +#include +#include +#endif + namespace autoware::pointcloud_preprocessor { class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node @@ -52,16 +58,34 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node public: explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); ~PointCloudConcatenateDataSynchronizerComponent() override = default; + + template + bool publish_clouds_preprocess( + const ConcatenatedCloudResult & concatenated_cloud_result); + + template + void publish_clouds_postprocess( + const ConcatenatedCloudResult & concatenated_cloud_result, + double reference_timestamp_min, double reference_timestamp_max); + + template void publish_clouds( - ConcatenatedCloudResult && concatenated_cloud_result, double reference_timestamp_min, - double reference_timestamp_max); - void delete_collector(CloudCollector & cloud_collector); - std::list> get_cloud_collectors(); - void add_cloud_collector(const std::shared_ptr & collector); + ConcatenatedCloudResult && concatenated_cloud_result, + double reference_timestamp_min, double reference_timestamp_max); + + template + void delete_collector(CloudCollector & cloud_collector); + + template + std::list>> get_cloud_collectors(); + + template + void add_cloud_collector(const std::shared_ptr> & collector); private: struct Parameters { + bool use_cuda; bool debug_mode; bool has_static_tf_only; bool rosbag_replay; @@ -88,8 +112,14 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node double diagnostic_reference_timestamp_max_{0.0}; std::unordered_map diagnostic_topic_to_original_stamp_map_; - std::shared_ptr combine_cloud_handler_; - std::list> cloud_collectors_; + std::shared_ptr combine_cloud_handler_; + std::list>> cloud_collectors_; + +#ifdef USE_CUDA + std::list>> + cuda_cloud_collectors_; +#endif + std::mutex cloud_collectors_mutex_; std::unordered_map topic_to_offset_map_; std::unordered_map topic_to_noise_window_map_; @@ -98,11 +128,24 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node static constexpr const char * default_sync_topic_postfix = "_synchronized"; // subscribers +#ifdef USE_CUDA + std::vector< + std::shared_ptr>> + cuda_pointcloud_subs_; +#endif std::vector::SharedPtr> pointcloud_subs_; rclcpp::Subscription::SharedPtr twist_sub_; rclcpp::Subscription::SharedPtr odom_sub_; // publishers +#ifdef USE_CUDA + std::shared_ptr> + cuda_concatenated_cloud_publisher_; + std::unordered_map< + std::string, + std::shared_ptr>> + topic_to_transformed_cuda_cloud_publisher_map_; +#endif rclcpp::Publisher::SharedPtr concatenated_cloud_publisher_; std::unordered_map::SharedPtr> topic_to_transformed_cloud_publisher_map_; @@ -111,8 +154,20 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::unique_ptr> stop_watch_ptr_; diagnostic_updater::Updater diagnostic_updater_{this}; + template + bool cloud_callback_preprocess( + const typename PointCloudMessage::ConstSharedPtr & input_ptr, const std::string & topic_name); + void cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + +#ifdef USE_CUDA + void cuda_cloud_callback( + const cuda_blackboard::CudaPointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); +#endif + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); 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 85e42b4291626..8764dc38c6412 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -31,6 +31,7 @@ autoware_point_types autoware_universe_utils cgal + cuda_blackboard cv_bridge diagnostic_updater image_transport 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 b0a7f746be778..277d89335eea6 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp @@ -19,15 +19,20 @@ #include +#ifdef USE_CUDA +#include +#endif + #include namespace autoware::pointcloud_preprocessor { -CloudCollector::CloudCollector( +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) + 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), @@ -42,19 +47,23 @@ CloudCollector::CloudCollector( std::bind(&CloudCollector::concatenate_callback, this)); } -void CloudCollector::set_reference_timestamp(double timestamp, double noise_window) +template +void CloudCollector::set_reference_timestamp( + double timestamp, double noise_window) { reference_timestamp_max_ = timestamp + noise_window; reference_timestamp_min_ = timestamp - noise_window; } -std::tuple CloudCollector::get_reference_timestamp_boundary() +template +std::tuple CloudCollector::get_reference_timestamp_boundary() { return std::make_tuple(reference_timestamp_min_, reference_timestamp_max_); } -void CloudCollector::process_pointcloud( - const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud) +template +void CloudCollector::process_pointcloud( + const std::string & topic_name, typename PointCloudMessage::ConstSharedPtr cloud) { // 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. @@ -71,7 +80,18 @@ void CloudCollector::process_pointcloud( } } -void CloudCollector::concatenate_callback() +template +void CloudCollector::concatenate_callback() +{ + static_assert( + std::is_same::value || + std::is_same::value, + "This function is only available for sensor_msgs::msg::PointCloud2 and " + "cuda_blackboard::CudaPointCloud2"); +} + +template <> +void CloudCollector::concatenate_callback() { if (debug_mode_) { auto time_until_trigger = timer_->time_until_trigger(); @@ -110,16 +130,68 @@ void CloudCollector::concatenate_callback() ros2_parent_node_->delete_collector(*this); } -ConcatenatedCloudResult CloudCollector::concatenate_pointclouds( - std::unordered_map topic_to_cloud_map) +#ifdef USE_CUDA + +template <> +void CloudCollector::concatenate_callback() +{ + if (debug_mode_) { + 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"; + + log_stream << "Collector's reference time min: " << reference_timestamp_min_ + << " to max: " << reference_timestamp_max_ << " 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()); + } + + // 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), reference_timestamp_min_, reference_timestamp_max_); + + ros2_parent_node_->delete_collector(*this); +} +#endif + +template +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() +template +std::unordered_map +CloudCollector::get_topic_to_cloud_map() { return topic_to_cloud_map_; } } // namespace autoware::pointcloud_preprocessor + +template class autoware::pointcloud_preprocessor::CloudCollector; + +#ifdef USE_CUDA +template class autoware::pointcloud_preprocessor::CloudCollector; +#endif 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 135ab943bebd4..a2e049c72711d 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,19 +14,58 @@ #include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" +#ifdef USE_CUDA +#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler_kernel.hpp" +#endif + #include #include +#ifdef USE_CUDA +#include + +#include +#endif + #include #include #include #include +#ifdef USE_CUDA + +#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); + +#endif + namespace autoware::pointcloud_preprocessor { -CombineCloudHandler::CombineCloudHandler( +CombineCloudHandlerBase::CombineCloudHandlerBase( rclcpp::Node & node, 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) @@ -41,7 +80,7 @@ CombineCloudHandler::CombineCloudHandler( { } -void CombineCloudHandler::process_twist( +void CombineCloudHandlerBase::process_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg) { geometry_msgs::msg::TwistStamped msg; @@ -68,7 +107,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; @@ -95,14 +134,91 @@ 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; +} + +CombineCloudHandler::CombineCloudHandler( + rclcpp::Node & node, 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) +{ +} + +void CombineCloudHandler::convert_to_xyzirc_cloud( + const typename sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud, + typename sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud) { xyzirc_cloud->header = input_cloud->header; @@ -125,14 +241,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; @@ -155,7 +271,7 @@ void CombineCloudHandler::convert_to_xyzirc_cloud( } } -void CombineCloudHandler::correct_pointcloud_motion( +void CombineCloudHandler::correct_pointcloud_motion( const std::shared_ptr & transformed_cloud_ptr, const std::vector & pc_stamps, std::unordered_map & transform_memo, @@ -181,10 +297,13 @@ 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) +/* template<> */ +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) { @@ -236,7 +355,7 @@ ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( 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_); @@ -264,71 +383,222 @@ 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) +#ifdef USE_CUDA + +CombineCloudHandler::CombineCloudHandler( + rclcpp::Node & node, 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) { - // 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(); + 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); } +} - 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; +void CombineCloudHandler::allocate_pointclouds() +{ + std::lock_guard lock(mutex_); - 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; + 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_); + } - 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(); + concatenated_cloud_ptr = std::make_unique(); + concatenated_cloud_ptr->data = cuda_blackboard::make_unique( + max_concat_pointcloud_size_ * input_topics_.size()); +} - 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; +ConcatenatedCloudResult +CombineCloudHandler::combine_pointclouds( + std::unordered_map & + 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; } - 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; + 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; + + cudaMemset(output_points + concatenated_start_index, 222, num_points); + + // Apply the kernel to the pointclouds + transform_launch( + reinterpret_cast(cloud->data.get()), num_points, transform_struct, + output_points + concatenated_start_index, stream); + concatenated_start_index += num_points; } - Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); + 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(); + } - float cos_yaw = std::cos(yaw); - float sin_yaw = std::sin(yaw); + concatenated_start_index = 0; - 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; + 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; - return transformation_matrix; + 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; } +#endif } // namespace autoware::pointcloud_preprocessor + +template class autoware::pointcloud_preprocessor::CombineCloudHandler< + sensor_msgs::msg::PointCloud2>; + +#ifdef USE_CUDA +template class autoware::pointcloud_preprocessor::CombineCloudHandler< + cuda_blackboard::CudaPointCloud2>; +#endif diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler_kernel.cu b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler_kernel.cu new file mode 100644 index 0000000000000..497d7e26f6459 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler_kernel.cu @@ -0,0 +1,57 @@ +// 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_kernel.hpp" + +#include + +#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) +{ + int threadsPerBlock = 256; + int blocksPerGrid = (num_points + threadsPerBlock - 1) / threadsPerBlock; + + transform_kernel<<>>( + input_points, num_points, transform, output_points); +} + +} // namespace autoware::pointcloud_preprocessor 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 219f391b58b22..8850ac7933606 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 @@ -44,6 +44,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro stop_watch_ptr_->tic("processing_time"); // initialize parameters + params_.use_cuda = declare_parameter("use_cuda"); params_.debug_mode = declare_parameter("debug_mode"); params_.has_static_tf_only = declare_parameter("has_static_tf_only"); params_.rosbag_replay = declare_parameter("rosbag_replay"); @@ -67,6 +68,12 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro params_.lidar_timestamp_noise_window = declare_parameter>("lidar_timestamp_noise_window"); +#ifndef USE_CUDA + if (use_cuda) { + throw std::runtime_error("Attempted to use CUDA functionalities but CUDA is not available."); + } +#endif + if (params_.input_topics.empty()) { throw std::runtime_error("Need a 'input_topics' parameter to be set before continuing."); } @@ -92,19 +99,38 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } // Publishers - concatenated_cloud_publisher_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); + if (!params_.use_cuda) { + concatenated_cloud_publisher_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); + + // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud + if (params_.publish_synchronized_pointcloud) { + for (auto & topic : params_.input_topics) { + std::string new_topic = + replace_sync_topic_name_postfix(topic, params_.synchronized_pointcloud_postfix); + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); + topic_to_transformed_cloud_publisher_map_.insert({topic, publisher}); + } + } + } + +#ifdef USE_CUDA + if (params_.use_cuda) { + cuda_concatenated_cloud_publisher_ = + std::make_shared>( + *this, "output"); - // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud - if (params_.publish_synchronized_pointcloud) { for (auto & topic : params_.input_topics) { std::string new_topic = replace_sync_topic_name_postfix(topic, params_.synchronized_pointcloud_postfix); - auto publisher = this->create_publisher( - new_topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); - topic_to_transformed_cloud_publisher_map_.insert({topic, publisher}); + auto publisher = std::make_shared< + cuda_blackboard::CudaBlackboardPublisher>( + *this, new_topic); + topic_to_transformed_cuda_cloud_publisher_map_.insert({topic, publisher}); } } +#endif // Subscribers if (params_.is_motion_compensated) { @@ -126,27 +152,59 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } } - for (const std::string & topic : params_.input_topics) { - std::function callback = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, - topic); + if (!params_.use_cuda) { + for (const std::string & topic : params_.input_topics) { + std::function callback = + std::bind( + &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, + std::placeholders::_1, 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); + 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_.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); } - // 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); +#ifdef USE_CUDA + if (params_.use_cuda) { + for (const std::string & topic : params_.input_topics) { + std::function callback = + std::bind( + &PointCloudConcatenateDataSynchronizerComponent::cuda_cloud_callback, this, + std::placeholders::_1, topic); + + auto cuda_pointcloud_sub = std::make_shared< + cuda_blackboard::CudaBlackboardSubscriber>( + *this, topic, false, callback); + cuda_pointcloud_subs_.push_back(cuda_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_.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); + } +#endif // Diagnostic Updater diagnostic_updater_.setHardwareID("concatenate_data_checker"); @@ -184,8 +242,9 @@ std::string PointCloudConcatenateDataSynchronizerComponent::replace_sync_topic_n return replaced_topic_name; } -void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) +template +bool PointCloudConcatenateDataSynchronizerComponent::cloud_callback_preprocess( + const typename PointCloudMessage::ConstSharedPtr & input_ptr, const std::string & topic_name) { stop_watch_ptr_->toc("processing_time", true); if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { @@ -198,7 +257,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); } - return; + return false; } if (params_.debug_mode) { @@ -209,9 +268,18 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( this->get_clock()->now().seconds() - rclcpp::Time(input_ptr->header.stamp).seconds()); } - if (input_ptr->data.empty()) { + if (input_ptr->width * input_ptr->height == 0) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + } + + return true; +} + +void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + if (!cloud_callback_preprocess(input_ptr, topic_name)) { return; } @@ -239,11 +307,14 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( } } - // if point cloud didn't find matched collector, create a new collector. + // if the point cloud didn't find a matching collector, create a new one. if (!collector_found) { - auto new_cloud_collector = std::make_shared( + auto combine_cloud_handler = + std::dynamic_pointer_cast>( + combine_cloud_handler_); + 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); + combine_cloud_handler, params_.input_topics.size(), params_.timeout_sec, params_.debug_mode); cloud_collectors_.push_back(new_cloud_collector); cloud_collectors_lock.unlock(); @@ -254,6 +325,58 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( } } +#ifdef USE_CUDA +void PointCloudConcatenateDataSynchronizerComponent::cuda_cloud_callback( + const cuda_blackboard::CudaPointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name) +{ + if (!cloud_callback_preprocess(input_ptr, topic_name)) { + return; + } + + // 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 + bool collector_found = false; + + if (!cuda_cloud_collectors_.empty()) { + for (const auto & cuda_cloud_collector : cuda_cloud_collectors_) { + auto [reference_timestamp_min, reference_timestamp_max] = + cuda_cloud_collector->get_reference_timestamp_boundary(); + + if ( + rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] < + reference_timestamp_max + topic_to_noise_window_map_[topic_name] && + rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name] > + reference_timestamp_min - topic_to_noise_window_map_[topic_name]) { + cloud_collectors_lock.unlock(); + cuda_cloud_collector->process_pointcloud(topic_name, input_ptr); + collector_found = true; + break; + } + } + } + + // if the point cloud didn't find a matching collector, create a new one. + if (!collector_found) { + auto combine_cloud_handler = + std::dynamic_pointer_cast>( + combine_cloud_handler_); + 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); + + cuda_cloud_collectors_.push_back(new_cloud_collector); + cloud_collectors_lock.unlock(); + new_cloud_collector->set_reference_timestamp( + rclcpp::Time(input_ptr->header.stamp).seconds() - topic_to_offset_map_[topic_name], + topic_to_noise_window_map_[topic_name]); + new_cloud_collector->process_pointcloud(topic_name, input_ptr); + } +} +#endif + void PointCloudConcatenateDataSynchronizerComponent::twist_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) { @@ -266,14 +389,14 @@ void PointCloudConcatenateDataSynchronizerComponent::odom_callback( combine_cloud_handler_->process_odometry(input); } -void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( - ConcatenatedCloudResult && concatenated_cloud_result, double reference_timestamp_min, - double reference_timestamp_max) +template +bool PointCloudConcatenateDataSynchronizerComponent::publish_clouds_preprocess( + const ConcatenatedCloudResult & concatenated_cloud_result) { // should never come to this state. if (concatenated_cloud_result.concatenate_cloud_ptr == nullptr) { RCLCPP_ERROR(this->get_logger(), "Concatenate cloud is a nullptr."); - return; + return false; } current_concatenate_cloud_timestamp_ = rclcpp::Time(concatenated_cloud_result.concatenate_cloud_ptr->header.stamp).seconds(); @@ -295,6 +418,45 @@ void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( publish_pointcloud_ = true; } + return true; +} + +template +void PointCloudConcatenateDataSynchronizerComponent::publish_clouds_postprocess( + const ConcatenatedCloudResult & concatenated_cloud_result, + double reference_timestamp_min, double reference_timestamp_max) +{ + diagnostic_reference_timestamp_min_ = reference_timestamp_min; + diagnostic_reference_timestamp_max_ = reference_timestamp_max; + 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 PointCloudConcatenateDataSynchronizerComponent::publish_clouds( + ConcatenatedCloudResult && concatenated_cloud_result, + double reference_timestamp_min, double reference_timestamp_max) +{ + if (!publish_clouds_preprocess(concatenated_cloud_result)) { + return; + } + if (publish_pointcloud_) { latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; auto concatenate_pointcloud_output = std::make_unique( @@ -323,30 +485,13 @@ void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( } } - diagnostic_reference_timestamp_min_ = reference_timestamp_min; - diagnostic_reference_timestamp_max_ = reference_timestamp_max; - 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); - } - } + publish_clouds_postprocess( + concatenated_cloud_result, reference_timestamp_min, reference_timestamp_max); } +template <> void PointCloudConcatenateDataSynchronizerComponent::delete_collector( - CloudCollector & cloud_collector) + CloudCollector & cloud_collector) { // protect cloud collectors list std::lock_guard cloud_collectors_lock(cloud_collectors_mutex_); @@ -354,7 +499,8 @@ void PointCloudConcatenateDataSynchronizerComponent::delete_collector( // change this to something else auto it = std::find_if( cloud_collectors_.begin(), cloud_collectors_.end(), - [&cloud_collector](const std::shared_ptr & collector) { + [&cloud_collector]( + const std::shared_ptr> & collector) { return collector.get() == &cloud_collector; }); if (it != cloud_collectors_.end()) { @@ -364,6 +510,77 @@ void PointCloudConcatenateDataSynchronizerComponent::delete_collector( } } +#ifdef USE_CUDA + +template <> +void PointCloudConcatenateDataSynchronizerComponent::publish_clouds< + cuda_blackboard::CudaPointCloud2>( + ConcatenatedCloudResult && concatenated_cloud_result, + double reference_timestamp_min, double reference_timestamp_max) +{ + if (!publish_clouds_preprocess(concatenated_cloud_result)) { + return; + } + + if (publish_pointcloud_) { + latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; + + cuda_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()) { + topic_to_transformed_cuda_cloud_publisher_map_[topic]->publish( + std::move((*concatenated_cloud_result.topic_to_transformed_cloud_map).at(topic))); + } else { + RCLCPP_WARN( + this->get_logger(), + "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", topic.c_str()); + } + } + } + } + + publish_clouds_postprocess( + concatenated_cloud_result, reference_timestamp_min, reference_timestamp_max); + + auto combine_cloud_handler = + std::dynamic_pointer_cast>( + combine_cloud_handler_); + + combine_cloud_handler->allocate_pointclouds(); +} + +template <> +void PointCloudConcatenateDataSynchronizerComponent::delete_collector( + CloudCollector & cuda_cloud_collector) +{ + // protect cloud collectors list + std::lock_guard cloud_collectors_lock(cloud_collectors_mutex_); + + // change this to something else + auto it = std::find_if( + cuda_cloud_collectors_.begin(), cuda_cloud_collectors_.end(), + [&cuda_cloud_collector]( + const std::shared_ptr> & collector) { + return collector.get() == &cuda_cloud_collector; + }); + if (it != cuda_cloud_collectors_.end()) { + cuda_cloud_collectors_.erase(it); + } else { + throw std::runtime_error("Try to delete a cloud_collector that is not in the cloud_collectors"); + } +} + +#endif + std::string PointCloudConcatenateDataSynchronizerComponent::format_timestamp(double timestamp) { std::ostringstream oss; @@ -428,18 +645,36 @@ void PointCloudConcatenateDataSynchronizerComponent::check_concat_status( } } -std::list> +template <> +std::list>> PointCloudConcatenateDataSynchronizerComponent::get_cloud_collectors() { return cloud_collectors_; } +template <> void PointCloudConcatenateDataSynchronizerComponent::add_cloud_collector( - const std::shared_ptr & collector) + const std::shared_ptr> & collector) { cloud_collectors_.push_back(collector); } +#ifdef USE_CUDA +template <> +std::list>> +PointCloudConcatenateDataSynchronizerComponent::get_cloud_collectors() +{ + return cuda_cloud_collectors_; +} + +template <> +void PointCloudConcatenateDataSynchronizerComponent::add_cloud_collector( + const std::shared_ptr> & collector) +{ + cuda_cloud_collectors_.push_back(collector); +} +#endif + } // namespace autoware::pointcloud_preprocessor #include diff --git a/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp index c1fd55943d13a..675833f9d8c90 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/utility/memory.cpp @@ -18,30 +18,31 @@ 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 +50,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 +98,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 +160,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 +227,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 bd22376b1c564..9c7218cd571aa 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp @@ -61,12 +61,13 @@ class ConcatenateCloudTest : public ::testing::Test concatenate_node_ = std::make_shared< autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( node_options); - combine_cloud_handler_ = - std::make_shared( - *concatenate_node_, std::vector{"lidar_top", "lidar_left", "lidar_right"}, - "base_link", true, true, true, false); + combine_cloud_handler_ = std::make_shared< + autoware::pointcloud_preprocessor::CombineCloudHandler>( + *concatenate_node_, std::vector{"lidar_top", "lidar_left", "lidar_right"}, + "base_link", true, true, true, false); - collector_ = std::make_shared( + collector_ = std::make_shared< + autoware::pointcloud_preprocessor::CloudCollector>( std::dynamic_pointer_cast< autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( concatenate_node_->shared_from_this()), @@ -168,8 +169,11 @@ class ConcatenateCloudTest : public ::testing::Test std::shared_ptr concatenate_node_; - std::shared_ptr combine_cloud_handler_; - std::shared_ptr collector_; + std::shared_ptr< + autoware::pointcloud_preprocessor::CombineCloudHandler> + combine_cloud_handler_; + std::shared_ptr> + collector_; std::shared_ptr tf_broadcaster_; static constexpr int32_t timestamp_seconds{10}; @@ -316,7 +320,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; @@ -448,7 +452,7 @@ TEST_F(ConcatenateCloudTest, TestDeleteCollector) { concatenate_node_->add_cloud_collector(collector_); concatenate_node_->delete_collector(*collector_); - EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); + EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); } TEST_F(ConcatenateCloudTest, TestProcessSingleCloud) @@ -464,14 +468,14 @@ TEST_F(ConcatenateCloudTest, TestProcessSingleCloud) auto topic_to_cloud_map = collector_->get_topic_to_cloud_map(); EXPECT_EQ(topic_to_cloud_map["lidar_top"], top_pointcloud_ptr); - EXPECT_FALSE(concatenate_node_->get_cloud_collectors().empty()); + EXPECT_FALSE(concatenate_node_->get_cloud_collectors().empty()); // Sleep for timeout seconds (200 ms) std::this_thread::sleep_for(std::chrono::milliseconds(200)); rclcpp::spin_some(concatenate_node_); // Collector should concatenate and publish the pointcloud, also delete itself. - EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); + EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); } TEST_F(ConcatenateCloudTest, TestProcessMultipleCloud) @@ -499,7 +503,7 @@ TEST_F(ConcatenateCloudTest, TestProcessMultipleCloud) collector_->process_pointcloud("lidar_left", left_pointcloud_ptr); collector_->process_pointcloud("lidar_right", right_pointcloud_ptr); - EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); + EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); } int main(int argc, char ** argv)