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 ff72e433e9602..bc295878bd2ac 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 @@ -318,6 +318,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( 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)); } } @@ -332,6 +335,9 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( // 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()); transformPointCloud(e.second, transformed_cloud_ptr); @@ -479,16 +485,16 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_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!"); - return; + } else { + // convert to XYZI pointcloud if pointcloud is not empty + convertToXYZICloud(input, xyzi_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_), 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 5d261f42391e9..8796477f3d9ae 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -274,6 +274,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() 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)); } } @@ -288,11 +291,22 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() // Step2. Calculate compensation transform and concatenate with the oldest stamp for (const auto & e : cloud_stdmap_) { if (e.second != nullptr) { + // transform pointcloud sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( + new sensor_msgs::msg::PointCloud2()); + if (e.second->data.size() == 0) { + // 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; + continue; + } + // transform pointcloud to output frame transformPointCloud(e.second, transformed_cloud_ptr); - // calculate transforms to oldest stamp + // calculate transforms to oldest stamp and transform pointcloud 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) { @@ -301,15 +315,15 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() 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); } @@ -414,7 +428,9 @@ void PointCloudDataSynchronizerComponent::cloud_callback( 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); + if (input->data.size() > 0) { + 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(