@@ -112,7 +112,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro
112
112
}
113
113
114
114
// Check if publish synchronized pointcloud
115
- publish_synchronized_pointcloud_ = declare_parameter (" publish_synchronized_pointcloud" , false );
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 );
116
118
synchronized_pointcloud_postfix_ =
117
119
declare_parameter (" synchronized_pointcloud_postfix" , " pointcloud" );
118
120
}
@@ -397,10 +399,23 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds(
397
399
pcl::concatenatePointCloud (
398
400
*concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr);
399
401
}
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;
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
+
404
419
} else {
405
420
not_subscribed_topic_names_.insert (e.first );
406
421
}
@@ -418,20 +433,6 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
418
433
const auto & transformed_raw_points =
419
434
PointCloudConcatenateDataSynchronizerComponent::combineClouds (concat_cloud_ptr);
420
435
421
- for (const auto & e : cloud_stdmap_) {
422
- if (e.second != nullptr ) {
423
- if (debug_publisher_) {
424
- const auto pipeline_latency_ms =
425
- std::chrono::duration<double , std::milli>(
426
- std::chrono::nanoseconds (
427
- (this ->get_clock ()->now () - e.second ->header .stamp ).nanoseconds ()))
428
- .count ();
429
- debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
430
- " debug" + e.first + " /pipeline_latency_ms" , pipeline_latency_ms);
431
- }
432
- }
433
- }
434
-
435
436
// publish concatenated pointcloud
436
437
if (concat_cloud_ptr) {
437
438
auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);
@@ -469,6 +470,19 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
469
470
debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
470
471
" debug/processing_time_ms" , processing_time_ms);
471
472
}
473
+ for (const auto & e : cloud_stdmap_) {
474
+ if (e.second != nullptr ) {
475
+ if (debug_publisher_) {
476
+ const auto pipeline_latency_ms =
477
+ std::chrono::duration<double , std::milli>(
478
+ std::chrono::nanoseconds (
479
+ (this ->get_clock ()->now () - e.second ->header .stamp ).nanoseconds ()))
480
+ .count ();
481
+ debug_publisher_->publish <tier4_debug_msgs::msg::Float64Stamped>(
482
+ " debug" + e.first + " /pipeline_latency_ms" , pipeline_latency_ms);
483
+ }
484
+ }
485
+ }
472
486
}
473
487
474
488
// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
0 commit comments