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 {