feat(pointcloud_preprocessor): enable to count empty pointclouds topi…

…c in concatenate pointclouds nodelet (autowarefoundation#6277)

* feat: enable to count empty pointclouds topic

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix: time sync function to adapt empty pc topic

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

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);
@@ -493,16 +499,16 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name)
 {
   std::lock_guard<std::mutex> lock(mutex_);
+  sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2());
   auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*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_),

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<std::mutex> lock(mutex_);
   auto input = std::make_shared<sensor_msgs::msg::PointCloud2>(*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(