feat(pointcloud_preprocessor): add pipeline_latency_ms debug publishe…

…r to missed modules (#6569) Signed-off-by: Berkay Karaman <brkay54@gmail.com>
autowarefoundation · Mar 14, 2024 · 6ee9627 · 6ee9627
1 parent ca46a8f
commit 6ee9627
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/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);

diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/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 {