From cbd3a41526492aa56aa2543ede5acb9a1b2ca68a Mon Sep 17 00:00:00 2001 From: mtlazaro Date: Thu, 9 Feb 2023 18:30:43 +0100 Subject: [PATCH] Publishing PointCloud2 with ROS timestamp --- livox_ros_driver/livox_ros_driver/lddc.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/livox_ros_driver/livox_ros_driver/lddc.cpp b/livox_ros_driver/livox_ros_driver/lddc.cpp index 55b18fd..56b07e0 100644 --- a/livox_ros_driver/livox_ros_driver/lddc.cpp +++ b/livox_ros_driver/livox_ros_driver/lddc.cpp @@ -203,7 +203,8 @@ uint32_t Lddc::PublishPointcloud2(LidarDataQueue *queue, uint32_t packet_num, } /** Use the first packet timestamp as pointcloud2 msg timestamp */ if (!published_packet) { - cloud.header.stamp = ros::Time(timestamp / 1000000000.0); + //cloud.header.stamp = ros::Time(timestamp / 1000000000.0); + cloud.header.stamp = ros::Time::now(); } uint32_t single_point_num = storage_packet.point_num * echo_num;