feat(pointcloud_preprocessor): make concatenate node to publish pointclouds in sensor frame (#6586)

* feat: make concatenate node to publish pointclouds in sensor frame

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* chore: disable frame transform if not necessary

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

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<std::string> not_subscribed_topic_names_;

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_;

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<double, std::milli>(
-            std::chrono::nanoseconds(
-              (this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
-            .count();
-        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
-          "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
-      }
-    }
-  }
-
   // publish concatenated pointcloud
   if (concat_cloud_ptr) {
     auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*concat_cloud_ptr);
@@ -469,6 +470,19 @@ void PointCloudConcatenateDataSynchronizerComponent::publish()
     debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "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<double, std::milli>(
+            std::chrono::nanoseconds(
+              (this->get_clock()->now() - e.second->header.stamp).nanoseconds()))
+            .count();
+        debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+          "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms);
+      }
+    }
+  }
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

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<std::string>(declare_parameter("output_frame", ""));
-    if (output_frame_.empty()) {
+    keep_input_frame_in_synchronized_pointcloud_ =
+      static_cast<bool>(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_;