From f83b0a1305be5aa46389402596de4f23f58a1a2f Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Fri, 10 May 2024 09:24:24 +0200 Subject: [PATCH] Fix timestamp: subtract packet time instead of full scan time The previous timestamp patch in PR #104 subtracted the full scan time from the last point time of the _first_ packet to determine the scan timestamp. It has to be the time for a single packet instead. Otherwise, the timestamp is too far in the past. This is especially noticeable when the scan frequency is in a lower range like 10Hz, i.e. the scan timestamp would be 100ms off compared to other sensor modalities. --- src/pf_driver/src/ros/pf_data_publisher.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/pf_driver/src/ros/pf_data_publisher.cpp b/src/pf_driver/src/ros/pf_data_publisher.cpp index a3e7a78b..eb209f54 100644 --- a/src/pf_driver/src/ros/pf_data_publisher.cpp +++ b/src/pf_driver/src/ros/pf_data_publisher.cpp @@ -68,11 +68,16 @@ void PFDataPublisher::to_msg_queue(T& packet, uint16_t layer_idx, int layer_incl msg->header.frame_id.assign(frame_id_); msg->header.seq = packet.header.header.scan_number; msg->scan_time = static_cast(scan_time.toSec()); - msg->header.stamp = packet.last_acquired_point_stamp - scan_time; msg->angle_increment = packet.header.angular_increment / 10000.0 * (M_PI / 180.0); { msg->time_increment = (params_->angular_fov * msg->scan_time) / (M_PI * 2.0) / packet.header.num_points_scan; + + // Set the timestamp of the full scan to the first scan point's time, + // as required by the ROS LaserScan message definition. + const auto packet_time = ros::Duration(msg->time_increment * packet.header.num_points_packet); + msg->header.stamp = packet.last_acquired_point_stamp - packet_time; + msg->angle_min = params_->angle_min; msg->angle_max = params_->angle_max; if (std::is_same::value) // Only Packet C1 for R2300