feat(pointcloud_preprocessor): add pipeline_latency_ms debug publisher to missed modules

sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -281,6 +281,20 @@
   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

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -374,6 +374,15 @@
   // 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 {