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 4374017d66d9b..60e53fe0aa9b8 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 @@ -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 index cd2b84c0b385d..cf4511ca22d70 100644 --- 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());