From dde84669b1302378bb98f58d1c98ba50a3bb684e Mon Sep 17 00:00:00 2001 From: vividf Date: Fri, 6 Dec 2024 13:43:58 +0900 Subject: [PATCH] chore: remove nodelet cpp Signed-off-by: vividf --- .../concatenate_and_time_sync_nodelet.cpp | 718 ------------------ 1 file changed, 718 deletions(-) delete mode 100644 sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp deleted file mode 100644 index f8baae3405873..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ /dev/null @@ -1,718 +0,0 @@ -// 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 "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" - -#include "autoware/pointcloud_preprocessor/utility/memory.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -#define DEFAULT_SYNC_TOPIC_POSTFIX \ - "_synchronized" // default postfix name for synchronized pointcloud - -////////////////////////////////////////////////////////////////////////////////////////////// - -namespace autoware::pointcloud_preprocessor -{ -PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( - const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options), - input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) -{ - // initialize debug tool - { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - } - - // 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; - } - has_static_tf_only_ = declare_parameter("has_static_tf_only", false); - 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; - } - - // Check if publish synchronized pointcloud - publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true); - keep_input_frame_in_synchronized_pointcloud_ = - declare_parameter("keep_input_frame_in_synchronized_pointcloud", true); - synchronized_pointcloud_postfix_ = - declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); - } - - // 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 - { - managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); - } - - // Output Publishers - { - rclcpp::PublisherOptions pub_options; - pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); - } - - // Subscribers - { - RCLCPP_DEBUG_STREAM( - get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); - for (const auto & input_topic : input_topics_) { - RCLCPP_DEBUG_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( - &PointCloudConcatenateDataSynchronizerComponent::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); - } - - if (input_twist_topic_type_ == "twist") { - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, - std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); - } else if (input_twist_topic_type_ == "odom") { - auto odom_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, - std::placeholders::_1); - sub_odom_ = this->create_subscription( - "~/input/odom", rclcpp::QoS{100}, odom_cb); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); - throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); - } - } - - // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud - if (publish_synchronized_pointcloud_) { - rclcpp::PublisherOptions pub_options; - pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - - for (auto & topic : input_topics_) { - std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_); - auto publisher = this->create_publisher( - new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); - 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(&PointCloudConcatenateDataSynchronizerComponent::timer_callback, this)); - } - - // Diagnostic Updater - { - updater_.setHardwareID("concatenate_data_checker"); - updater_.add( - "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus); - } -} - -std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix( - const std::string & original_topic_name, const std::string & postfix) -{ - std::string replaced_topic_name; - // separate the topic name by '/' and replace the last element with the new postfix - size_t pos = original_topic_name.find_last_of("/"); - if (pos == std::string::npos) { - // not found '/': this is not a namespaced topic - RCLCPP_WARN_STREAM( - get_logger(), - "The topic name is not namespaced. The postfix will be added to the end of the topic name."); - return original_topic_name + postfix; - } else { - // replace the last element with the new postfix - replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; - } - - // if topic name is the same with original topic name, add postfix to the end of the topic name - if (replaced_topic_name == original_topic_name) { - RCLCPP_WARN_STREAM( - get_logger(), "The topic name " - << original_topic_name - << " have the same postfix with synchronized pointcloud. We use the postfix " - "to the end of the topic name."); - replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX; - } - return replaced_topic_name; -} - -/** - * @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 - if (twist_ptr_queue_.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), - "No twist is available. Please confirm twist topic and timestamp"); - return Eigen::Matrix4f::Identity(); - } - - // return identity if old_stamp is newer than new_stamp - if (old_stamp > new_stamp) { - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), - "old_stamp is newer than 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 -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) { - if (e.second->data.size() == 0) { - continue; - } - 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) { - if (e.second->data.size() == 0) { - continue; - } - sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( - new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud(output_frame_, *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); - - // concatenate - if (concat_cloud_ptr == nullptr) { - 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); - } - // convert to original sensor frame if necessary - bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_); - if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { - sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame( - new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud( - e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, - *transformed_cloud_ptr_in_sensor_frame); - transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; - transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id; - transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame; - } else { - transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; - transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; - transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; - } - - } else { - not_subscribed_topic_names_.insert(e.first); - } - } - concat_cloud_ptr->header.stamp = oldest_stamp; - return transformed_clouds; -} - -void PointCloudConcatenateDataSynchronizerComponent::publish() -{ - stop_watch_ptr_->toc("processing_time", true); - sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; - not_subscribed_topic_names_.clear(); - - 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)); - } else { - RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); - } - - // publish transformed raw pointclouds - 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()); - } - } - } - - 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); - } - for (const auto & e : cloud_stdmap_) { - if (e.second != nullptr) { - if (debug_publisher_) { - const auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); - } - } - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( - 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_valid_intensity = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; - }); - - bool has_valid_return_type = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; - }); - - bool has_valid_channel = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; - }); - - 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_valid_intensity && has_valid_return_type && has_valid_channel) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); - sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { - PointXYZIRC point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - point.intensity = *it_i; - point.return_type = *it_r; - point.channel = *it_c; - output_modifier.push_back(std::move(point)); - } - } else { - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZIRC point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - output_modifier.push_back(std::move(point)); - } - } -} - -void PointCloudConcatenateDataSynchronizerComponent::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 PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) -{ - if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); - - if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), - "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); - } - - return; - } - - std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); - auto input = std::make_shared(*input_ptr); - if (input->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } else { - // convert to XYZIRC pointcloud if pointcloud is not empty - convertToXYZIRCCloud(input, xyzirc_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] = xyzirc_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] = xyzirc_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 PointCloudConcatenateDataSynchronizerComponent::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 PointCloudConcatenateDataSynchronizerComponent::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::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 = input->header; - twist_ptr->twist = input->twist.twist; - twist_ptr_queue_.push_back(twist_ptr); -} - -void PointCloudConcatenateDataSynchronizerComponent::odom_callback( - const nav_msgs::msg::Odometry::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 = input->header; - twist_ptr->twist = input->twist.twist; - twist_ptr_queue_.push_back(twist_ptr); -} - -void PointCloudConcatenateDataSynchronizerComponent::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 autoware::pointcloud_preprocessor - -#include -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent)