Skip to content

Commit a0fb9e0

Browse files
committed
chore: disable frame transform if not necessary
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 855d440 commit a0fb9e0

File tree

2 files changed

+5
-3
lines changed

2 files changed

+5
-3
lines changed

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -400,7 +400,8 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
400400
*concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
401401
}
402402
// convert to original sensor frame if necessary
403-
if (keep_input_frame_in_synchronized_pointcloud_) {
403+
bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_);
404+
if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
404405
sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame(
405406
new sensor_msgs::msg::PointCloud2());
406407
pcl_ros::transformPointCloud(

sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -353,8 +353,9 @@ PointCloudDataSynchronizerComponent::synchronizeClouds()
353353
pcl_ros::transformPointCloud(
354354
adjust_to_old_data_transform, *transformed_cloud_ptr,
355355
*transformed_delay_compensated_cloud_ptr);
356-
357-
if (keep_input_frame_in_synchronized_pointcloud_) {
356+
// transform to sensor frame if needed
357+
bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_);
358+
if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) {
358359
sensor_msgs::msg::PointCloud2::SharedPtr
359360
transformed_delay_compensated_cloud_ptr_in_input_frame(
360361
new sensor_msgs::msg::PointCloud2());

0 commit comments

Comments
 (0)