feat(pointcloud_preprocessor): make concatenate node to publish pointclouds in sensor frame (autowarefoundation#6586)

* feat: make concatenate node to publish pointclouds in sensor frame

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

* chore: disable frame transform if not necessary

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

---------

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

Author: YoshiRi

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp

@@ -126,6 +126,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   double timeout_sec_ = 0.1;
 
   bool publish_synchronized_pointcloud_;
+  bool keep_input_frame_in_synchronized_pointcloud_;
   std::string synchronized_pointcloud_postfix_;
 
   std::set<std::string> not_subscribed_topic_names_;

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

@@ -112,7 +112,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
     }
 
     // Check if publish synchronized pointcloud
-    publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
+    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");
   }
@@ -397,10 +399,23 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
         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;
+      // 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());
+        pcl_ros::transformPointCloud(
+          (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
+          *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_);
+        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);
     }