From 9e72d847b9f9ec6501d4f89c7221b16e5cae977f Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Tue, 9 Jul 2024 15:48:45 +0900
Subject: [PATCH] temp: instrumentation for pointcloud time delay

---
 nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp | 1 +
 nebula_ros/src/hesai/decoder_wrapper.cpp                | 6 ++++++
 2 files changed, 7 insertions(+)

diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp
index 2dd5f12c8..6e78814a9 100644
--- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp
+++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp
@@ -67,6 +67,7 @@ class HesaiDecoderWrapper
 
   nebula::Status status_;
   rclcpp::Logger logger_;
+  rclcpp::Node & parent_node_;
 
   const std::shared_ptr<nebula::drivers::HesaiHwInterface> hw_interface_;
   std::shared_ptr<const nebula::drivers::HesaiSensorConfiguration> sensor_cfg_;
diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp
index ad5ec60ed..87c84473c 100644
--- a/nebula_ros/src/hesai/decoder_wrapper.cpp
+++ b/nebula_ros/src/hesai/decoder_wrapper.cpp
@@ -3,6 +3,8 @@
 #include "nebula_ros/hesai/decoder_wrapper.hpp"
 
 #include <nebula_common/hesai/hesai_common.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/time.hpp>
 
 #include <memory>
 
@@ -20,6 +22,7 @@ HesaiDecoderWrapper::HesaiDecoderWrapper(
   const std::shared_ptr<const drivers::HesaiCalibrationConfigurationBase> & calibration)
 : status_(nebula::Status::NOT_INITIALIZED),
   logger_(parent_node->get_logger().get_child("HesaiDecoder")),
+  parent_node_(*parent_node),
   sensor_cfg_(config),
   calibration_cfg_ptr_(calibration)
 {
@@ -127,6 +130,9 @@ void HesaiDecoderWrapper::ProcessCloudPacket(
 
   cloud_watchdog_->update();
 
+  auto sys_pc_time_diff = parent_node_.now().seconds() - std::get<1>(pointcloud_ts);
+  RCLCPP_INFO_STREAM(logger_, "#DIFF(s) = " << std::setprecision(6) << std::setfill('0') << std::setw(9) << sys_pc_time_diff);
+
   // Publish scan message only if it has been written to
   if (current_scan_msg_ && !current_scan_msg_->packets.empty()) {
     packets_pub_->publish(std::move(current_scan_msg_));