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 hw_interface_; std::shared_ptr 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 +#include +#include #include @@ -20,6 +22,7 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( const std::shared_ptr & 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_));