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 {