feat(pointcloud_preprocessor): add pipeline_latency_ms debug publishe…

…r to missed modules (autowarefoundation#6569)

Signed-off-by: Berkay Karaman <[email protected]>

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp

@@ -281,6 +281,20 @@ void PointCloudConcatenationComponent::publish()
   checkSyncStatus();
   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);

sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp

@@ -374,6 +374,15 @@ void PointCloudDataSynchronizerComponent::publish()
   // publish transformed raw pointclouds
   for (const auto & e : transformed_raw_points) {
     if (e.second) {
+      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);
+      }
       auto output = std::make_unique<sensor_msgs::msg::PointCloud2>(*e.second);
       transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output));
     } else {