Skip to content

Commit 23d6755

Browse files
YoshiRiN-Eiki
authored andcommitted
git cherry-pick 44e4be7 # feat(pointcloud_preprocessor): enable to change synchronized pointcloud topic name (autowarefoundation#6525)
fix : fix : fix conflict occured by cherry pick Signed-off-by: N-Eiki <eiki.nagata.2@tier4.jp>
1 parent 39f09fd commit 23d6755

File tree

2 files changed

+5
-21
lines changed

2 files changed

+5
-21
lines changed

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

-1
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,6 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
126126
double timeout_sec_ = 0.1;
127127

128128
bool publish_synchronized_pointcloud_;
129-
bool keep_input_frame_in_synchronized_pointcloud_;
130129
std::string synchronized_pointcloud_postfix_;
131130

132131
std::set<std::string> not_subscribed_topic_names_;

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp

+5-20
Original file line numberDiff line numberDiff line change
@@ -112,9 +112,7 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
112112
}
113113

114114
// Check if publish synchronized pointcloud
115-
publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true);
116-
keep_input_frame_in_synchronized_pointcloud_ =
117-
declare_parameter("keep_input_frame_in_synchronized_pointcloud", true);
115+
publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false);
118116
synchronized_pointcloud_postfix_ =
119117
declare_parameter("synchronized_pointcloud_postfix", "pointcloud");
120118
}
@@ -399,23 +397,10 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
399397
pcl::concatenatePointCloud(
400398
*concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
401399
}
402-
// convert to original sensor frame if necessary
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) {
405-
sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame(
406-
new sensor_msgs::msg::PointCloud2());
407-
pcl_ros::transformPointCloud(
408-
(std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr,
409-
*transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_);
410-
transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp;
411-
transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id;
412-
transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame;
413-
} else {
414-
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
415-
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
416-
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
417-
}
418-
400+
// gather transformed clouds
401+
transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp;
402+
transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;
403+
transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr;
419404
} else {
420405
not_subscribed_topic_names_.insert(e.first);
421406
}

0 commit comments

Comments
 (0)