From 122c2e32fa82477bcca8cab8febef2305ad97669 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 4 Apr 2023 03:52:04 +0900 Subject: [PATCH 01/13] feat: fix concatenate pc compensation order Signed-off-by: yoshiri --- .../concatenate_data_nodelet.hpp | 6 +- .../concatenate_data_nodelet.cpp | 89 +++++++++++-------- 2 files changed, 57 insertions(+), 38 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index 77cfbe4610bf1..59b6976911324 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -151,9 +151,9 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::map offset_map_; void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - void combineClouds( - const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, - PointCloud2::SharedPtr & out); + Eigen::Matrix4f calcDelayCompensateTransform( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); void convertToXYZICloud( diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index d16323b7fc602..5afaf730027ed 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -201,17 +201,14 @@ void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( } } -void PointCloudConcatenateDataSynchronizerComponent::combineClouds( - const PointCloud2::ConstSharedPtr & in1, const PointCloud2::ConstSharedPtr & in2, - PointCloud2::SharedPtr & out) +Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::calcDelayCompensateTransform( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - if (twist_ptr_queue_.empty()) { - pcl::concatenatePointCloud(*in1, *in2, *out); - out->header.stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); - return; + // return identity if no twist is available or old_stamp is newer than new_stamp + if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + return Eigen::Matrix4f::Identity(); } - const auto old_stamp = std::min(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); auto old_twist_ptr_it = std::lower_bound( std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { @@ -220,7 +217,6 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds( old_twist_ptr_it = old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; - const auto new_stamp = std::max(rclcpp::Time(in1->header.stamp), rclcpp::Time(in2->header.stamp)); auto new_twist_ptr_it = std::lower_bound( std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { @@ -258,50 +254,73 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds( Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); Eigen::Translation3f translation(x, y, 0); Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); - - // TODO(YamatoAndo): if output_frame_ is not base_link, we must transform - - if (rclcpp::Time(in1->header.stamp) > rclcpp::Time(in2->header.stamp)) { - sensor_msgs::msg::PointCloud2::SharedPtr in1_t(new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud(rotation_matrix, *in1, *in1_t); - pcl::concatenatePointCloud(*in1_t, *in2, *out); - out->header.stamp = in2->header.stamp; - } else { - sensor_msgs::msg::PointCloud2::SharedPtr in2_t(new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud(rotation_matrix, *in2, *in2_t); - pcl::concatenatePointCloud(*in1, *in2_t, *out); - out->header.stamp = in1->header.stamp; - } + return rotation_matrix; } -void PointCloudConcatenateDataSynchronizerComponent::publish() +void PointCloudConcatenateDataSynchronizerComponent::combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) { - stop_watch_ptr_->toc("processing_time", true); - sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr_ = nullptr; - not_subscribed_topic_names_.clear(); + // Step1. gather stamps and sort it + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); + } + } + if (pc_stamps.empty()) { + return; + } + // sort stamps and get latest stamp + std::sort(pc_stamps.begin(), pc_stamps.end()); + const auto latest_stamp = pc_stamps.back(); + // Step2. Calculate compensation transform and concatenate with the latest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); transformPointCloud(e.second, transformed_cloud_ptr); - if (concat_cloud_ptr_ == nullptr) { - concat_cloud_ptr_ = transformed_cloud_ptr; - } else { - PointCloudConcatenateDataSynchronizerComponent::combineClouds( - concat_cloud_ptr_, transformed_cloud_ptr, concat_cloud_ptr_); + + // calculate transforms to latest stamp + Eigen::Matrix4f delay_compensation_matrix = Eigen::Matrix4f::Identity(); + for (const auto & stamp : pc_stamps) { + const auto compensate_delay = calcDelayCompensateTransform(e.second->header.stamp, stamp); + delay_compensation_matrix = compensate_delay * delay_compensation_matrix; } + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud( + delay_compensation_matrix, *transformed_cloud_ptr, + *transformed_delay_compensated_cloud_ptr); + // concatenate + if (concat_cloud_ptr == nullptr) { + concat_cloud_ptr = transformed_delay_compensated_cloud_ptr; + } else { + pcl::concatenatePointCloud( + *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); + } } else { not_subscribed_topic_names_.insert(e.first); } } + concat_cloud_ptr->header.stamp = latest_stamp; + return; +} + +void PointCloudConcatenateDataSynchronizerComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; + not_subscribed_topic_names_.clear(); + + PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); - if (concat_cloud_ptr_) { - auto output = std::make_unique(*concat_cloud_ptr_); + if (concat_cloud_ptr) { + auto output = std::make_unique(*concat_cloud_ptr); pub_output_->publish(std::move(output)); } else { - RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr_ is nullptr, skipping pointcloud publish."); + RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); } updater_.force_update(); From 0a92b8f1ccd743ee4a1cfd46ed8fc3e72dc6ea87 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 4 Apr 2023 04:37:39 +0900 Subject: [PATCH 02/13] feat: add transformed raw pc output to concat func Signed-off-by: yoshiri --- .../concatenate_data_nodelet.hpp | 6 ++- .../concatenate_data_nodelet.cpp | 41 ++++++++++++++++--- 2 files changed, 41 insertions(+), 6 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index 59b6976911324..51488c3730dfe 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -114,6 +114,9 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node private: /** \brief The output PointCloud publisher. */ rclcpp::Publisher::SharedPtr pub_output_; + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; /** \brief The maximum number of messages that we can store in the queue. */ int maximum_queue_size_ = 3; @@ -153,7 +156,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); Eigen::Matrix4f calcDelayCompensateTransform( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); - void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); + std::map combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); void publish(); void convertToXYZICloud( diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 5afaf730027ed..e3dd0ac8f67ee 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -128,7 +128,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro tf2_listener_ = std::make_shared(*tf2_buffer_); } - // Publishers + // Output Publishers { pub_output_ = this->create_publisher( "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); @@ -165,6 +165,16 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); } + // Transformed Raw PointCloud2 Publisher + { + for (auto & topic : input_topics_) { + std::string new_topic = topic + "_transformed"; + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + transformed_raw_pc_publisher_map_.insert({topic, publisher}); + } + } + // Set timer { const auto period_ns = std::chrono::duration_cast( @@ -257,18 +267,23 @@ Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::calcDelayCompens return rotation_matrix; } -void PointCloudConcatenateDataSynchronizerComponent::combineClouds( +std::map +PointCloudConcatenateDataSynchronizerComponent::combineClouds( sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) { + // map for storing the transformed point clouds + std::map transformed_clouds; + // Step1. gather stamps and sort it std::vector pc_stamps; for (const auto & e : cloud_stdmap_) { + transformed_clouds[e.first] = nullptr; if (e.second != nullptr) { pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); } } if (pc_stamps.empty()) { - return; + return transformed_clouds; } // sort stamps and get latest stamp std::sort(pc_stamps.begin(), pc_stamps.end()); @@ -300,12 +315,14 @@ void PointCloudConcatenateDataSynchronizerComponent::combineClouds( pcl::concatenatePointCloud( *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); } + // gather transformed clouds + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; } else { not_subscribed_topic_names_.insert(e.first); } } concat_cloud_ptr->header.stamp = latest_stamp; - return; + return transformed_clouds; } void PointCloudConcatenateDataSynchronizerComponent::publish() @@ -314,8 +331,10 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; not_subscribed_topic_names_.clear(); - PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + const auto & transformed_raw_points = + PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); pub_output_->publish(std::move(output)); @@ -323,6 +342,18 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); } + // publish transformed raw pointclouds + for (const auto & e : transformed_raw_points) { + if (e.second) { + auto output = std::make_unique(*e.second); + transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); + } else { + RCLCPP_WARN( + this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", + e.first.c_str()); + } + } + updater_.force_update(); cloud_stdmap_ = cloud_stdmap_tmp_; From 3ac7a89c9357852736ffdf747242a90ac3e9092d Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 6 Apr 2023 14:32:41 +0900 Subject: [PATCH 03/13] fix: concat pc to sync with oldest time stamp Signed-off-by: yoshiri --- .../concatenate_data_nodelet.hpp | 4 +- .../concatenate_data_nodelet.cpp | 37 +++++++++++++------ 2 files changed, 28 insertions(+), 13 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index 51488c3730dfe..a1e0b01bf4feb 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -154,7 +154,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::map offset_map_; void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); - Eigen::Matrix4f calcDelayCompensateTransform( + Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); std::map combineClouds( sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index e3dd0ac8f67ee..029de21bb43be 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -211,7 +211,15 @@ void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( } } -Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::calcDelayCompensateTransform( +/** + * @brief compute transform to adjust for old timestamp + * + * @param old_stamp + * @param new_stamp + * @return Eigen::Matrix4f: transformation matrix from new_stamp to old_stamp + */ +Eigen::Matrix4f +PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { // return identity if no twist is available or old_stamp is newer than new_stamp @@ -285,43 +293,50 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( if (pc_stamps.empty()) { return transformed_clouds; } - // sort stamps and get latest stamp + // sort stamps and get oldest stamp std::sort(pc_stamps.begin(), pc_stamps.end()); - const auto latest_stamp = pc_stamps.back(); + std::reverse(pc_stamps.begin(), pc_stamps.end()); + const auto oldest_stamp = pc_stamps.back(); - // Step2. Calculate compensation transform and concatenate with the latest stamp + // Step2. Calculate compensation transform and concatenate with the oldest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); transformPointCloud(e.second, transformed_cloud_ptr); - // calculate transforms to latest stamp - Eigen::Matrix4f delay_compensation_matrix = Eigen::Matrix4f::Identity(); + // calculate transforms to oldest stamp + Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); + rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); for (const auto & stamp : pc_stamps) { - const auto compensate_delay = calcDelayCompensateTransform(e.second->header.stamp, stamp); - delay_compensation_matrix = compensate_delay * delay_compensation_matrix; + const auto new_to_old_transform = + computeTransformToAdjustForOldTimestamp(stamp, transformed_stamp); + adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; + transformed_stamp = std::min(transformed_stamp, stamp); } sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( new sensor_msgs::msg::PointCloud2()); pcl_ros::transformPointCloud( - delay_compensation_matrix, *transformed_cloud_ptr, + adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); // concatenate if (concat_cloud_ptr == nullptr) { - concat_cloud_ptr = transformed_delay_compensated_cloud_ptr; + concat_cloud_ptr = + std::make_shared(*transformed_delay_compensated_cloud_ptr); } else { pcl::concatenatePointCloud( *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); } // gather transformed clouds + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; } else { not_subscribed_topic_names_.insert(e.first); } } - concat_cloud_ptr->header.stamp = latest_stamp; + concat_cloud_ptr->header.stamp = oldest_stamp; return transformed_clouds; } From 1bce3a86fad23248b5caf56ea60ee2a2da65fc85 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 6 Apr 2023 21:34:53 +0900 Subject: [PATCH 04/13] feat: add time sync node component Signed-off-by: yoshiri --- .../pointcloud_preprocessor/CMakeLists.txt | 6 + .../time_synchronizer_nodelet.hpp | 183 ++++++ .../time_synchronizer_nodelet.cpp | 553 ++++++++++++++++++ 3 files changed, 742 insertions(+) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp create mode 100644 sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index d8d561a9c5098..f01961591aaef 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -44,6 +44,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_data_nodelet.cpp + src/time_synchronizer/time_synchronizer_nodelet.cpp src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp src/downsample_filter/random_downsample_filter_nodelet.cpp @@ -70,6 +71,11 @@ target_link_libraries(pointcloud_preprocessor_filter ${PCL_LIBRARIES} ) +# ========== Time synchronizer ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointCloudDataSynchronizerComponent" + EXECUTABLE time_synchronizer_node) + # ========== Concatenate data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp new file mode 100644 index 0000000000000..19c96d8e1c462 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -0,0 +1,183 @@ +// Copyright 2023 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: TIME_SYNCHRONIZER.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "autoware_point_types/types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; +/** \brief @b PointCloudDataSynchronizerComponent is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudDataSynchronizerComponent : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + /** \brief constructor. */ + explicit PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); + + /** \brief constructor. + * \param queue_size the maximum queue size + */ + PointCloudDataSynchronizerComponent(const rclcpp::NodeOptions & node_options, int queue_size); + + /** \brief Empty destructor. */ + virtual ~PointCloudDataSynchronizerComponent() {} + +private: + /** \brief The output PointCloud publisher. */ + rclcpp::Publisher::SharedPtr pub_output_; + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_ = 3; + + double timeout_sec_ = 0.1; + + std::set not_subscribed_topic_names_; + + /** \brief A vector of subscriber. */ + std::vector::SharedPtr> filters_; + + rclcpp::Subscription::SharedPtr sub_twist_; + + rclcpp::TimerBase::SharedPtr timer_; + diagnostic_updater::Updater updater_{this}; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + // XmlRpc::XmlRpcValue input_topics_; + std::vector input_topics_; + + /** \brief TF listener object. */ + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + std::deque twist_ptr_queue_; + + std::map cloud_stdmap_; + std::map cloud_stdmap_tmp_; + std::mutex mutex_; + + std::vector input_offset_; + std::map offset_map_; + + void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); + void transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame); + Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + std::map synchronizeClouds(); + void publish(); + + void convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); + void setPeriod(const int64_t new_period); + void cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); + void timer_callback(); + + void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__TIME_SYNCHRONIZER__TIME_SYNCHRONIZER_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp new file mode 100644 index 0000000000000..40d32aafd65be --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -0,0 +1,553 @@ +// Copyright 2020 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#include "pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pointcloud_preprocessor +{ +PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_time_synchronizer_component", node_options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "time_synchronizer"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Set parameters + { + output_frame_ = static_cast(declare_parameter("output_frame", "")); + if (output_frame_.empty()) { + RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); + return; + } + declare_parameter("input_topics", std::vector()); + input_topics_ = get_parameter("input_topics").as_string_array(); + if (input_topics_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); + return; + } + if (input_topics_.size() == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Optional parameters + maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); + + input_offset_ = declare_parameter("input_offset", std::vector{}); + if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); + return; + } + } + + // Initialize not_subscribed_topic_names_ + { + for (const std::string & e : input_topics_) { + not_subscribed_topic_names_.insert(e); + } + } + + // Initialize offset map + { + for (size_t i = 0; i < input_offset_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_[i]; + } + } + + // tf2 listener + { + tf2_buffer_ = std::make_shared(this->get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + } + + // Output Publishers + { + pub_output_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + } + + // Subscribers + { + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (auto & input_topic : input_topics_) { + RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + } + + // Subscribe to the filters + filters_.resize(input_topics_.size()); + + // First input_topics_.size () filters are valid + for (size_t d = 0; d < input_topics_.size(); ++d) { + cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); + cloud_stdmap_tmp_ = cloud_stdmap_; + + // CAN'T use auto type here. + std::function cb = std::bind( + &PointCloudDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, + input_topics_[d]); + + filters_[d].reset(); + filters_[d] = this->create_subscription( + input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); + } + auto twist_cb = + std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); + sub_twist_ = this->create_subscription( + "/vehicle/status/velocity_status", rclcpp::QoS{100}, twist_cb); + } + + // Transformed Raw PointCloud2 Publisher + { + for (auto & topic : input_topics_) { + std::string new_topic = topic + "_transformed"; + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + transformed_raw_pc_publisher_map_.insert({topic, publisher}); + } + } + + // Set timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&PointCloudDataSynchronizerComponent::timer_callback, this)); + } + + // Diagnostic Updater + { + updater_.setHardwareID("concatenate_data_checker"); + updater_.add("concat_status", this, &PointCloudDataSynchronizerComponent::checkConcatStatus); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +// overloaded functions +void PointCloudDataSynchronizerComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) +{ + transformPointCloud(in, out, output_frame_); +} + +void PointCloudDataSynchronizerComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame) +{ + // Transform the point clouds into the specified output frame + if (target_frame != in->header.frame_id) { + // TODO(YamatoAndo): use TF2 + if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[transformPointCloud] Error converting first input dataset from %s to %s.", + in->header.frame_id.c_str(), target_frame.c_str()); + return; + } + } else { + out = std::make_shared(*in); + } +} + +/** + * @brief compute transform to adjust for old timestamp + * + * @param old_stamp + * @param new_stamp + * @return Eigen::Matrix4f: transformation matrix from new_stamp to old_stamp + */ +Eigen::Matrix4f PointCloudDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) +{ + // return identity if no twist is available or old_stamp is newer than new_stamp + if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + return Eigen::Matrix4f::Identity(); + } + + auto old_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + old_twist_ptr_it = + old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; + + auto new_twist_ptr_it = std::lower_bound( + std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, + [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { + return rclcpp::Time(x_ptr->header.stamp) < t; + }); + new_twist_ptr_it = + new_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : new_twist_ptr_it; + + auto prev_time = old_stamp; + double x = 0.0; + double y = 0.0; + double yaw = 0.0; + for (auto twist_ptr_it = old_twist_ptr_it; twist_ptr_it != new_twist_ptr_it + 1; ++twist_ptr_it) { + const double dt = + (twist_ptr_it != new_twist_ptr_it) + ? (rclcpp::Time((*twist_ptr_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( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " + "timestamp"); + break; + } + + const double dis = (*twist_ptr_it)->twist.linear.x * dt; + yaw += (*twist_ptr_it)->twist.angular.z * dt; + x += dis * std::cos(yaw); + y += dis * std::sin(yaw); + prev_time = (*twist_ptr_it)->header.stamp; + } + Eigen::AngleAxisf rotation_x(0, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf rotation_y(0, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); + Eigen::Translation3f translation(x, y, 0); + Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); + return rotation_matrix; +} + +std::map +PointCloudDataSynchronizerComponent::synchronizeClouds() +{ + // map for storing the transformed point clouds + std::map transformed_clouds; + + // Step1. gather stamps and sort it + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + transformed_clouds[e.first] = nullptr; + if (e.second != nullptr) { + pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); + } + } + if (pc_stamps.empty()) { + return transformed_clouds; + } + // sort stamps and get oldest stamp + std::sort(pc_stamps.begin(), pc_stamps.end()); + std::reverse(pc_stamps.begin(), pc_stamps.end()); + const auto oldest_stamp = pc_stamps.back(); + + // Step2. Calculate compensation transform and concatenate with the oldest stamp + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud(e.second, transformed_cloud_ptr); + + // calculate transforms to oldest stamp + Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); + rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); + for (const auto & stamp : pc_stamps) { + const auto new_to_old_transform = + computeTransformToAdjustForOldTimestamp(stamp, transformed_stamp); + adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; + transformed_stamp = std::min(transformed_stamp, stamp); + } + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud( + adjust_to_old_data_transform, *transformed_cloud_ptr, + *transformed_delay_compensated_cloud_ptr); + // gather transformed clouds + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + } else { + not_subscribed_topic_names_.insert(e.first); + } + } + return transformed_clouds; +} + +void PointCloudDataSynchronizerComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + not_subscribed_topic_names_.clear(); + + const auto & transformed_raw_points = PointCloudDataSynchronizerComponent::synchronizeClouds(); + + // publish transformed raw pointclouds + for (const auto & e : transformed_raw_points) { + if (e.second) { + auto output = std::make_unique(*e.second); + transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); + } else { + RCLCPP_WARN( + this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", + e.first.c_str()); + } + } + + updater_.force_update(); + + cloud_stdmap_ = cloud_stdmap_tmp_; + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + // 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); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudDataSynchronizerComponent::convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) +{ + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } + } +} + +void PointCloudDataSynchronizerComponent::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +void PointCloudDataSynchronizerComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + auto input = std::make_shared(*input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input, xyzi_input_ptr); + + const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_already_subscribed_this) { + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } else { + cloud_stdmap_[topic_name] = xyzi_input_ptr; + + const bool is_subscribed_all = std::all_of( + std::begin(cloud_stdmap_), std::end(cloud_stdmap_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_subscribed_all) { + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + cloud_stdmap_[e.first] = e.second; + } + } + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +void PointCloudDataSynchronizerComponent::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +void PointCloudDataSynchronizerComponent::twist_callback( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header.stamp = input->header.stamp; + twist_ptr->twist.linear.x = input->longitudinal_velocity; + twist_ptr->twist.linear.y = input->lateral_velocity; + twist_ptr->twist.angular.z = input->heading_rate; + twist_ptr_queue_.push_back(twist_ptr); +} + +void PointCloudDataSynchronizerComponent::checkConcatStatus( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + for (const std::string & e : input_topics_) { + const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + stat.add(e, subscribe_status); + } + + const int8_t level = not_subscribed_topic_names_.empty() + ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::WARN; + const std::string message = not_subscribed_topic_names_.empty() + ? "Concatenate all topics" + : "Some topics are not concatenated"; + stat.summary(level, message); +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudDataSynchronizerComponent) From e64d35d9f3757e04011218119480fc78e6ea8070 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 7 Apr 2023 12:44:23 +0900 Subject: [PATCH 05/13] refactor: time synchronizer class Signed-off-by: yoshiri --- .../time_synchronizer_nodelet.hpp | 5 +- .../time_synchronizer_nodelet.cpp | 56 +++++-------------- 2 files changed, 15 insertions(+), 46 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 19c96d8e1c462..09e24bedeb6b9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -93,6 +93,7 @@ using point_cloud_msg_wrapper::PointCloud2Modifier; * checks their timestamps, and concatenates their fields together into a single * PointCloud output message. * \author Radu Bogdan Rusu + * \edited by Yoshi Ri */ class PointCloudDataSynchronizerComponent : public rclcpp::Node { @@ -111,8 +112,6 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node virtual ~PointCloudDataSynchronizerComponent() {} private: - /** \brief The output PointCloud publisher. */ - rclcpp::Publisher::SharedPtr pub_output_; /** \brief Delay Compensated PointCloud publisher*/ std::map::SharedPtr> transformed_raw_pc_publisher_map_; @@ -171,7 +170,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); void timer_callback(); - void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 40d32aafd65be..a6bf64b835f61 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -13,40 +13,12 @@ // limitations under the License. /* - * Software License Agreement (BSD License) + * @brief PointCloudDataSynchronizerComponent class * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * subscribe: pointclouds, twists + * publish: timestamp "synchronized" pointclouds * + * @author Yoshi Ri */ #include "pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp" @@ -61,6 +33,8 @@ #include #include +// postfix for output topics +#define POSTFIX_NAME "_synchronized" ////////////////////////////////////////////////////////////////////////////////////////////// namespace pointcloud_preprocessor @@ -128,12 +102,6 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( tf2_listener_ = std::make_shared(*tf2_buffer_); } - // Output Publishers - { - pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); - } - // Subscribers { RCLCPP_INFO_STREAM( @@ -159,6 +127,8 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( filters_[d] = this->create_subscription( input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); } + + // Subscribe to the twist auto twist_cb = std::bind(&PointCloudDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); sub_twist_ = this->create_subscription( @@ -168,7 +138,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Transformed Raw PointCloud2 Publisher { for (auto & topic : input_topics_) { - std::string new_topic = topic + "_transformed"; + std::string new_topic = topic + POSTFIX_NAME; auto publisher = this->create_publisher( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); @@ -186,8 +156,8 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Diagnostic Updater { - updater_.setHardwareID("concatenate_data_checker"); - updater_.add("concat_status", this, &PointCloudDataSynchronizerComponent::checkConcatStatus); + updater_.setHardwareID("synchronize_data_checker"); + updater_.add("concat_status", this, &PointCloudDataSynchronizerComponent::checkSyncStatus); } } @@ -531,7 +501,7 @@ void PointCloudDataSynchronizerComponent::twist_callback( twist_ptr_queue_.push_back(twist_ptr); } -void PointCloudDataSynchronizerComponent::checkConcatStatus( +void PointCloudDataSynchronizerComponent::checkSyncStatus( diagnostic_updater::DiagnosticStatusWrapper & stat) { for (const std::string & e : input_topics_) { From 7d542238cc2b860bd7b4fcceb5122dc5684a81b7 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 7 Apr 2023 12:45:19 +0900 Subject: [PATCH 06/13] feat: add pc concatenate node Signed-off-by: yoshiri --- .../pointcloud_preprocessor/CMakeLists.txt | 6 + .../concatenate_pointclouds.hpp | 181 +++++++ .../concatenate_pointclouds.cpp | 468 ++++++++++++++++++ 3 files changed, 655 insertions(+) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp create mode 100644 sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index f01961591aaef..624ea2d775d3f 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -44,6 +44,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_data_nodelet.cpp + src/concatenate_data/concatenate_pointclouds.cpp src/time_synchronizer/time_synchronizer_nodelet.cpp src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -77,6 +78,11 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter EXECUTABLE time_synchronizer_node) # ========== Concatenate data ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "pointcloud_preprocessor::PointCloudConcatenationComponent" + EXECUTABLE concatenate_pointclouds_node) + +# ========== Concatenate and Sync data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" EXECUTABLE concatenate_data_node) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp new file mode 100644 index 0000000000000..f46d6595b2a76 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -0,0 +1,181 @@ +// Copyright 2023 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. + +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ + * + */ + +#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ +#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "autoware_point_types/types.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace pointcloud_preprocessor +{ +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; +/** \brief @b PointCloudConcatenationComponent is a special form of data + * synchronizer: it listens for a set of input PointCloud messages on the same topic, + * checks their timestamps, and concatenates their fields together into a single + * PointCloud output message. + * \author Radu Bogdan Rusu + */ +class PointCloudConcatenationComponent : public rclcpp::Node +{ +public: + typedef sensor_msgs::msg::PointCloud2 PointCloud2; + + /** \brief constructor. */ + explicit PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options); + + /** \brief constructor. + * \param queue_size the maximum queue size + */ + PointCloudConcatenationComponent(const rclcpp::NodeOptions & node_options, int queue_size); + + /** \brief Empty destructor. */ + virtual ~PointCloudConcatenationComponent() {} + +private: + /** \brief The output PointCloud publisher. */ + rclcpp::Publisher::SharedPtr pub_output_; + /** \brief Delay Compensated PointCloud publisher*/ + std::map::SharedPtr> + transformed_raw_pc_publisher_map_; + + /** \brief The maximum number of messages that we can store in the queue. */ + int maximum_queue_size_ = 3; + + double timeout_sec_ = 0.1; + + std::set not_subscribed_topic_names_; + + /** \brief A vector of subscriber. */ + std::vector::SharedPtr> filters_; + + rclcpp::Subscription::SharedPtr sub_twist_; + + rclcpp::TimerBase::SharedPtr timer_; + diagnostic_updater::Updater updater_{this}; + + /** \brief Output TF frame the concatenated points should be transformed to. */ + std::string output_frame_; + + /** \brief Input point cloud topics. */ + // XmlRpc::XmlRpcValue input_topics_; + std::vector input_topics_; + + /** \brief TF listener object. */ + std::shared_ptr tf2_buffer_; + std::shared_ptr tf2_listener_; + + std::deque twist_ptr_queue_; + + std::map cloud_stdmap_; + std::map cloud_stdmap_tmp_; + std::mutex mutex_; + + std::vector input_offset_; + std::map offset_map_; + + void transformPointCloud(const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out); + void transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame); + void checkSyncStatus(); + void combineClouds(sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); + void publish(); + + void convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); + void setPeriod(const int64_t new_period); + void cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, + const std::string & topic_name); + void timer_callback(); + + void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** \brief processing time publisher. **/ + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; +}; + +} // namespace pointcloud_preprocessor + +#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_POINTCLOUDS_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp new file mode 100644 index 0000000000000..4e26adb19bda5 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -0,0 +1,468 @@ +// Copyright 2023 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 "pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp" + +#include + +#include + +#include +#include +#include +#include +#include + +// postfix for input topics +#define POSTFIX_NAME "_synchronized" + +////////////////////////////////////////////////////////////////////////////////////////////// + +namespace pointcloud_preprocessor +{ +PointCloudConcatenationComponent::PointCloudConcatenationComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_concatenator_component", node_options) +{ + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "concatenate_pointclouds_debug"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Set parameters + { + output_frame_ = static_cast(declare_parameter("output_frame", "")); + if (output_frame_.empty()) { + RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); + return; + } + declare_parameter("input_topics", std::vector()); + input_topics_ = get_parameter("input_topics").as_string_array(); + if (input_topics_.empty()) { + RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); + return; + } + if (input_topics_.size() == 1) { + RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); + return; + } + + // Optional parameters + maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); + /** input pointclouds should be */ + timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.033)); + + input_offset_ = declare_parameter("input_offset", std::vector{}); + if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { + RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); + return; + } + } + // add postfix to topic names + { + for (auto & topic : input_topics_) { + topic = topic + POSTFIX_NAME; + } + } + + // Initialize not_subscribed_topic_names_ + { + for (const std::string & e : input_topics_) { + not_subscribed_topic_names_.insert(e); + } + } + + // Initialize offset map + { + for (size_t i = 0; i < input_offset_.size(); ++i) { + offset_map_[input_topics_[i]] = input_offset_[i]; + } + } + + // tf2 listener + { + tf2_buffer_ = std::make_shared(this->get_clock()); + tf2_listener_ = std::make_shared(*tf2_buffer_); + } + + // Output Publishers + { + pub_output_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); + } + + // Subscribers + { + RCLCPP_INFO_STREAM( + get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); + for (auto & input_topic : input_topics_) { + RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + } + + // Subscribe to the filters + filters_.resize(input_topics_.size()); + + // First input_topics_.size () filters are valid + for (size_t d = 0; d < input_topics_.size(); ++d) { + cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); + cloud_stdmap_tmp_ = cloud_stdmap_; + + // CAN'T use auto type here. + std::function cb = std::bind( + &PointCloudConcatenationComponent::cloud_callback, this, std::placeholders::_1, + input_topics_[d]); + + filters_[d].reset(); + filters_[d] = this->create_subscription( + input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); + } + } + + // Set timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&PointCloudConcatenationComponent::timer_callback, this)); + } + + // Diagnostic Updater + { + updater_.setHardwareID("concatenate_pc_checker"); + updater_.add("concat_status", this, &PointCloudConcatenationComponent::checkConcatStatus); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudConcatenationComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out) +{ + transformPointCloud(in, out, output_frame_); +} + +void PointCloudConcatenationComponent::transformPointCloud( + const PointCloud2::ConstSharedPtr & in, PointCloud2::SharedPtr & out, + const std::string & target_frame) +{ + // Transform the point clouds into the specified output frame + if (target_frame != in->header.frame_id) { + // TODO(YamatoAndo): use TF2 + if (!pcl_ros::transformPointCloud(target_frame, *in, *out, *tf2_buffer_)) { + RCLCPP_ERROR( + this->get_logger(), + "[transformPointCloud] Error converting first input dataset from %s to %s.", + in->header.frame_id.c_str(), target_frame.c_str()); + return; + } + } else { + out = std::make_shared(*in); + } +} + +void PointCloudConcatenationComponent::checkSyncStatus() +{ + // gather the stamps + std::vector pc_stamps; + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + const auto stamp = rclcpp::Time(e.second->header.stamp); + auto it = std::find(pc_stamps.begin(), pc_stamps.end(), stamp); + if (it != pc_stamps.end()) { + // found + continue; + } else { + // not found + pc_stamps.push_back(stamp); + } + } + } + + // 1. Check if all stamps are same + if (pc_stamps.size() == 1) { + return; + } + // else, do the same for the tmp cloud + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + const auto stamp = rclcpp::Time(e.second->header.stamp); + auto it = std::find(pc_stamps.begin(), pc_stamps.end(), stamp); + if (it != pc_stamps.end()) { + // found + continue; + } else { + // not found + pc_stamps.push_back(stamp); + } + } + } + // sort pc_stamps + std::sort(pc_stamps.begin(), pc_stamps.end()); + // restrict the size of pc_stamps to newer 2 stamps + if (pc_stamps.size() > 2) { + pc_stamps.erase(pc_stamps.begin(), pc_stamps.end() - 2); + } + + // 2. if the stamp variation is 2, return true and reshape the cloud_stdmap_tmp_ + for (auto & e : cloud_stdmap_) { + // if the cloud is nullptr, check if the tmp cloud is not nullptr and has the same stamp + if (e.second == nullptr) { + // do nothing + } else { + // else if cloud is not nullptr + const auto current_stamp = rclcpp::Time(e.second->header.stamp); + if (current_stamp == pc_stamps.front()) { + // if the stamp is the oldest one, do nothing + } else if (current_stamp == pc_stamps.back()) { + // if the stamp is the newest one, move the cloud to the tmp cloud + cloud_stdmap_tmp_[e.first] = e.second; + e.second = nullptr; + } else { + // this state should not be reached. discard data + e.second = nullptr; + } + } + // check for the tmp cloud + if (cloud_stdmap_tmp_[e.first] == nullptr) { + continue; + } + const auto next_stamp = rclcpp::Time(cloud_stdmap_tmp_[e.first]->header.stamp); + if (next_stamp == pc_stamps.front()) { + e.second = cloud_stdmap_tmp_[e.first]; + cloud_stdmap_tmp_[e.first] = nullptr; + } else if (next_stamp == pc_stamps.back()) { + // do nothing + } else { + // this state should not be reached. discard data + cloud_stdmap_tmp_[e.first] = nullptr; + } + } + return; +} + +void PointCloudConcatenationComponent::combineClouds( + sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) +{ + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + // transform to output frame + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud(e.second, transformed_cloud_ptr); + + // concatenate + if (concat_cloud_ptr == nullptr) { + concat_cloud_ptr = std::make_shared(*transformed_cloud_ptr); + } else { + pcl::concatenatePointCloud(*concat_cloud_ptr, *transformed_cloud_ptr, *concat_cloud_ptr); + } + } else { + not_subscribed_topic_names_.insert(e.first); + } + } +} + +void PointCloudConcatenationComponent::publish() +{ + stop_watch_ptr_->toc("processing_time", true); + sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; + not_subscribed_topic_names_.clear(); + + checkSyncStatus(); + combineClouds(concat_cloud_ptr); + + // publish concatenated pointcloud + if (concat_cloud_ptr) { + auto output = std::make_unique(*concat_cloud_ptr); + pub_output_->publish(std::move(output)); + } else { + RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); + } + + updater_.force_update(); + + // update cloud_stdmap_ + cloud_stdmap_ = cloud_stdmap_tmp_; + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + // 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); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void PointCloudConcatenationComponent::convertToXYZICloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) +{ + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } + } +} + +void PointCloudConcatenationComponent::setPeriod(const int64_t new_period) +{ + if (!timer_) { + return; + } + int64_t old_period = 0; + rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); + } + ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); + } +} + +void PointCloudConcatenationComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) +{ + std::lock_guard lock(mutex_); + auto input = std::make_shared(*input_ptr); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input, xyzi_input_ptr); + + const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); + const bool is_already_subscribed_tmp = std::any_of( + std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_already_subscribed_this) { + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; + + if (!is_already_subscribed_tmp) { + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } else { + cloud_stdmap_[topic_name] = xyzi_input_ptr; + + const bool is_subscribed_all = std::all_of( + std::begin(cloud_stdmap_), std::end(cloud_stdmap_), + [](const auto & e) { return e.second != nullptr; }); + + if (is_subscribed_all) { + for (const auto & e : cloud_stdmap_tmp_) { + if (e.second != nullptr) { + cloud_stdmap_[e.first] = e.second; + } + } + std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { + e.second = nullptr; + }); + + timer_->cancel(); + publish(); + } else if (offset_map_.size() > 0) { + timer_->cancel(); + auto period = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); + try { + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } + } +} + +void PointCloudConcatenationComponent::timer_callback() +{ + using std::chrono_literals::operator""ms; + timer_->cancel(); + if (mutex_.try_lock()) { + publish(); + mutex_.unlock(); + } else { + try { + std::chrono::nanoseconds period = 10ms; + setPeriod(period.count()); + } catch (rclcpp::exceptions::RCLError & ex) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); + } + timer_->reset(); + } +} + +void PointCloudConcatenationComponent::checkConcatStatus( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + for (const std::string & e : input_topics_) { + const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; + stat.add(e, subscribe_status); + } + + const int8_t level = not_subscribed_topic_names_.empty() + ? diagnostic_msgs::msg::DiagnosticStatus::OK + : diagnostic_msgs::msg::DiagnosticStatus::WARN; + const std::string message = not_subscribed_topic_names_.empty() + ? "Concatenate all topics" + : "Some topics are not concatenated"; + stat.summary(level, message); +} +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(pointcloud_preprocessor::PointCloudConcatenationComponent) From 575484b2fb8e3ba1266eae3fe41ec14b3b3903f8 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 7 Apr 2023 19:07:09 +0900 Subject: [PATCH 07/13] rename existing concat node file Signed-off-by: yoshiri --- sensing/pointcloud_preprocessor/CMakeLists.txt | 2 +- ...ta_nodelet.hpp => concatenate_and_time_sync_nodelet.hpp} | 6 +++--- ...ta_nodelet.cpp => concatenate_and_time_sync_nodelet.cpp} | 6 ++++-- 3 files changed, 8 insertions(+), 6 deletions(-) rename sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/{concatenate_data_nodelet.hpp => concatenate_and_time_sync_nodelet.hpp} (97%) rename sensing/pointcloud_preprocessor/src/concatenate_data/{concatenate_data_nodelet.cpp => concatenate_and_time_sync_nodelet.cpp} (99%) diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 624ea2d775d3f..9491a8000141d 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -43,7 +43,7 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp - src/concatenate_data/concatenate_data_nodelet.cpp + src/concatenate_data/concatenate_and_time_sync_nodelet.cpp src/concatenate_data/concatenate_pointclouds.cpp src/time_synchronizer/time_synchronizer_nodelet.cpp src/crop_box_filter/crop_box_filter_nodelet.cpp diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp similarity index 97% rename from sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp rename to sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index a1e0b01bf4feb..cf2fb96f92674 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -49,8 +49,8 @@ * */ -#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ -#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ +#ifndef POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ +#define POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ #include #include @@ -179,4 +179,4 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node } // namespace pointcloud_preprocessor -#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ +#endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp similarity index 99% rename from sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp rename to sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index 029de21bb43be..b25555500dfe3 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -49,7 +49,7 @@ * */ -#include "pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp" +#include "pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" #include @@ -61,6 +61,8 @@ #include #include +#define POSTFIX_NAME "_synchronized" + ////////////////////////////////////////////////////////////////////////////////////////////// namespace pointcloud_preprocessor @@ -168,7 +170,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Transformed Raw PointCloud2 Publisher { for (auto & topic : input_topics_) { - std::string new_topic = topic + "_transformed"; + std::string new_topic = topic + POSTFIX_NAME; auto publisher = this->create_publisher( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); From ab1fbfcd4674d63dde0fd2b517011afac56278fc Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 7 Apr 2023 19:15:40 +0900 Subject: [PATCH 08/13] update concat_node launch to choose two methods Signed-off-by: yoshiri --- .../launch/preprocessor.launch.py | 61 ++++++++++++++----- 1 file changed, 47 insertions(+), 14 deletions(-) diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 4b3fac1baa65f..d0ef7008ab279 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -40,19 +40,52 @@ def generate_launch_description(): tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") # set concat filter as a component - concat_component = ComposableNode( - package=pkg, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_filter", - remappings=[("output", "points_raw/concatenated")], - parameters=[ - { - "input_topics": LaunchConfiguration("input_points_raw_list"), - "output_frame": LaunchConfiguration("tf_output_frame"), - "approximate_sync": True, - } - ], - ) + # if you want to use single node, set use_single_node = True + use_single_node = False + if use_single_node: + sync_and_concat_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="sync_and_concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) + concat_components = [sync_and_concat_component] + else: + time_sync_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent", + name="synchronizer_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) + + concat_component = ComposableNode( + package=pkg, + plugin="pointcloud_preprocessor::PointCloudConcatenationComponent", + name="concatenate_filter", + remappings=[("output", "points_raw/concatenated")], + parameters=[ + { + "input_topics": LaunchConfiguration("input_points_raw_list"), + "output_frame": LaunchConfiguration("tf_output_frame"), + "approximate_sync": True, + } + ], + ) + concat_components = [time_sync_component, concat_component] # set crop box filter as a component cropbox_component = ComposableNode( @@ -93,7 +126,7 @@ def generate_launch_description(): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[concat_component, cropbox_component], + composable_node_descriptions=concat_components + [cropbox_component], output="screen", ) From d5915a604ce1ec048dd6c8a4686f3e4d7cfe406c Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 29 May 2023 02:49:13 +0900 Subject: [PATCH 09/13] update readme usage explanation Signed-off-by: yoshiri --- .../docs/concatenate-data.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index b48c476390aed..79d73abd0d212 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -41,6 +41,19 @@ The figure below represents the reception time of each sensor data and how it is | ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | +## Actual Usage + +For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file. + +### Node separation options for advanced users + +Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes. + +The main advantage of this separation is that the user can gain access to the "time synchronized pointclouds" before the concatenation process. +It will be useful for othrer pointcloud based logics, such as occupancy grid map generation. + +This node separation causes a slight delay and increase in memory usage ([see this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)), users who do not have intend to use the "time synchronized pointclouds" may not have to use this option. + ## Assumptions / Known limits It is necessary to assume that the vehicle odometry value exists, the sensor data and odometry timestamp are correct, and the TF from `base_link` to `sensor_frame` is also correct. From 4311150ea1a944a006f8341aa322a99423da858e Mon Sep 17 00:00:00 2001 From: yoshiri Date: Wed, 31 May 2023 19:23:52 +0900 Subject: [PATCH 10/13] add parameter to control publisher of the synched pointclouds Signed-off-by: yoshiri --- .../concatenate_and_time_sync_nodelet.hpp | 2 ++ .../concatenate_and_time_sync_nodelet.cpp | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index cf2fb96f92674..2a06a87eb8147 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -123,6 +123,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node double timeout_sec_ = 0.1; + bool publish_synchronized_pointcloud_; + std::set not_subscribed_topic_names_; /** \brief A vector of subscriber. */ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index b25555500dfe3..036652255e4e9 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -108,6 +108,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); return; } + + // Check if publish synchronized pointcloud + publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false); } // Initialize not_subscribed_topic_names_ @@ -360,14 +363,16 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } // publish transformed raw pointclouds - for (const auto & e : transformed_raw_points) { - if (e.second) { - auto output = std::make_unique(*e.second); - transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); - } else { - RCLCPP_WARN( - this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", - e.first.c_str()); + if (publish_synchronized_pointcloud_) { + for (const auto & e : transformed_raw_points) { + if (e.second) { + auto output = std::make_unique(*e.second); + transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); + } else { + RCLCPP_WARN( + this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", + e.first.c_str()); + } } } From 3d390b5b7c19bdbe2c3b9b9ac701533a4c47a9a1 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Wed, 31 May 2023 20:37:42 +0900 Subject: [PATCH 11/13] Fix readme for pc concatenation Signed-off-by: yoshiri --- .../docs/concatenate-data.md | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index 79d73abd0d212..4bd8ba1959c2d 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -37,22 +37,36 @@ The figure below represents the reception time of each sensor data and how it is ### Core Parameters -| Name | Type | Default Value | Description | -| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | +| Name | Type | Default Value | Description | +| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | +| `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | +| `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | ## Actual Usage For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file. -### Node separation options for advanced users +### How to tuning timeout_sec and input_offset -Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes. +The values in `timeout_sec` and `input_offset` are used in the timercallback to control concatenation timings. + +- Assumptions + - when the timer runs out, we concatenate the pointclouds in the buffer + - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec` + - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset` + - we assume all lidar has same frequency + +| Name | Description | How to tune | +| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `timeout_sec` | timeout sec for default timer | To avoid mis-concatenation, at least this value must be shorter than sampling time. | +| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. | -The main advantage of this separation is that the user can gain access to the "time synchronized pointclouds" before the concatenation process. -It will be useful for othrer pointcloud based logics, such as occupancy grid map generation. +### Node separation options for future + +Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes. -This node separation causes a slight delay and increase in memory usage ([see this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)), users who do not have intend to use the "time synchronized pointclouds" may not have to use this option. +In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)). ## Assumptions / Known limits From ee7bad22f6c809b370c58f81bf0fe412707158fc Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 14 Jul 2023 18:21:55 +0900 Subject: [PATCH 12/13] make node separation flag controllable by input argument Signed-off-by: yoshiri --- .../launch/preprocessor.launch.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index d0ef7008ab279..f04e85f12895f 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -40,9 +40,12 @@ def generate_launch_description(): tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") # set concat filter as a component - # if you want to use single node, set use_single_node = True - use_single_node = False - if use_single_node: + separate_concatenate_node_and_timesync_node = DeclareLaunchArgument( + "separate_concatenate_node_and_timesync_node", + default_value="False", + description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", + ) + if separate_concatenate_node_and_timesync_node: sync_and_concat_component = ComposableNode( package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", From a5d14078133647d4e2aa96f4949a8ce9575f1222 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Tue, 18 Jul 2023 00:57:46 +0900 Subject: [PATCH 13/13] fix node separation arguments Signed-off-by: yoshiri --- .../launch/preprocessor.launch.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index ef401d2783696..cdff4bf0162a4 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -40,12 +40,16 @@ def generate_launch_description(): tf_output_frame_param = DeclareLaunchArgument("tf_output_frame", default_value="base_link") # set concat filter as a component - separate_concatenate_node_and_timesync_node = DeclareLaunchArgument( + separate_concatenate_node_and_timesync_node_str = DeclareLaunchArgument( "separate_concatenate_node_and_timesync_node", - default_value="False", + default_value="false", description="Set True to separate concatenate node and timesync node. which will cause to larger memory usage.", ) - if separate_concatenate_node_and_timesync_node: + separate_concatenate_node_and_timesync_node = ( + separate_concatenate_node_and_timesync_node_str.lower() == "true" + ) + + if not separate_concatenate_node_and_timesync_node: sync_and_concat_component = ComposableNode( package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", @@ -56,6 +60,7 @@ def generate_launch_description(): "input_topics": LaunchConfiguration("input_points_raw_list"), "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, + "publish_synchronized_pointcloud": False, } ], )