chore: disable frame transform if not necessary

Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Mar 13, 2024 · a0fb9e0 · a0fb9e0
1 parent 855d440
commit a0fb9e0
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
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
@@ -400,7 +400,8 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
           *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
       }
       // convert to original sensor frame if necessary
-      if (keep_input_frame_in_synchronized_pointcloud_) {
+      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(

diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp
@@ -353,8 +353,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
       pcl_ros::transformPointCloud(
         adjust_to_old_data_transform, *transformed_cloud_ptr,
         *transformed_delay_compensated_cloud_ptr);
-
-      if (keep_input_frame_in_synchronized_pointcloud_) {
+      // transform to sensor frame if needed
+      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_delay_compensated_cloud_ptr_in_input_frame(
             new sensor_msgs::msg::PointCloud2());