diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index d1560cccd0b15..84b056bec37b7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -126,6 +126,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node double timeout_sec_ = 0.1; bool publish_synchronized_pointcloud_; + bool keep_input_frame_in_synchronized_pointcloud_; std::string synchronized_pointcloud_postfix_; std::set not_subscribed_topic_names_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 4a36c32980b0e..9330b1e998b5b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -138,6 +138,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; + bool keep_input_frame_in_synchronized_pointcloud_; /** \brief Input point cloud topics. */ // XmlRpc::XmlRpcValue input_topics_; 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 f6407c532b5f8..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 @@ -112,7 +112,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } // Check if publish synchronized pointcloud - publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false); + publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true); + keep_input_frame_in_synchronized_pointcloud_ = + declare_parameter("keep_input_frame_in_synchronized_pointcloud", true); synchronized_pointcloud_postfix_ = declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); } @@ -397,10 +399,23 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( pcl::concatenatePointCloud( *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); } - // gather transformed clouds - transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; - transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; - transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + // convert to original sensor frame if necessary + 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( + (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, + *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_); + transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; + transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id; + transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame; + } else { + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + } + } else { not_subscribed_topic_names_.insert(e.first); } @@ -418,20 +433,6 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() const auto & transformed_raw_points = PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); - for (const auto & e : cloud_stdmap_) { - if (e.second != nullptr) { - if (debug_publisher_) { - const auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); - } - } - } - // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); @@ -469,6 +470,19 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 8f02721a67cfb..cf4511ca22d70 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -58,7 +58,9 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( std::string synchronized_pointcloud_postfix; { output_frame_ = static_cast(declare_parameter("output_frame", "")); - if (output_frame_.empty()) { + keep_input_frame_in_synchronized_pointcloud_ = + static_cast(declare_parameter("keep_input_frame_in_synchronized_pointcloud", false)); + if (output_frame_.empty() && !keep_input_frame_in_synchronized_pointcloud_) { RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); return; } @@ -351,7 +353,18 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() pcl_ros::transformPointCloud( adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); - + // 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()); + transformPointCloud( + transformed_delay_compensated_cloud_ptr, + transformed_delay_compensated_cloud_ptr_in_input_frame, e.second->header.frame_id); + transformed_delay_compensated_cloud_ptr = + transformed_delay_compensated_cloud_ptr_in_input_frame; + } // gather transformed clouds transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_;