Incorrect time stamp of laser scan message

[mintar]

The published timestamp of the LaserScan messages is slightly incorrect. According to the LaserScan message specification, the timestamp in the header is the acquisition time of the first ray in the scan. However, the timestamp published by the driver is (at least in some cases) something after the last ray of the scan.

Bug description

Simplified, the problem is this (warning, pseudocode):

# current implementation (incorrect):
msg.header.stamp = time_message_received

# correct implementation:
msg.header.stamp = time_message_received
                   - len(msg.ranges) * msg.time_increment   // shift backward to time of first scan point

Example where this happens:

• If the parameter timestamp_mode == "TIME_FROM_ROS_RECEPTION", then _use_ros_time = true:

ros2_ouster_drivers/ros2_ouster/src/ouster_driver.cpp

Lines 92 to 98 in b619a88

	if (lidar_config.timestamp_mode == "TIME_FROM_ROS_RECEPTION") {
	RCLCPP_WARN(
	this->get_logger(),
	"Using TIME_FROM_ROS_RECEPTION to stamp data with ROS time on "
	"reception. This has unmodelled latency!");
	lidar_config.timestamp_mode = "TIME_FROM_INTERNAL_OSC";
	_use_ros_time = true;

• ... and then override_ts = this->now().nanoseconds():

ros2_ouster_drivers/ros2_ouster/src/ouster_driver.cpp

Line 231 in b619a88

uint64_t override_ts = this->_use_ros_time ? this->now().nanoseconds() : 0;

• ... and then override_ts is used as the time stamp without subtracting len(msg.ranges) * msg.time_increment:

ros2_ouster_drivers/ros2_ouster/include/ros2_ouster/conversions.hpp

Line 220 in b619a88

msg.header.stamp = override_ts == 0 ? t : rclcpp::Time(override_ts);

I have not checked timestamp_modes other than TIME_FROM_ROS_RECEPTION; how to correctly handle these depends on whether the Ouster reports time stamps relative to the first ray, last ray or whatever, and I haven't looked into this.

Consequences of the bug

RViz and any other node that processes the laser scans to assemble them into a point cloud, perform SLAM etc. will now compute the time stamps of the laser points incorrectly. For example, the time stamp of the last ray will be computed as msg.header.stamp + len(msg.ranges) * msg.time_increment, thereby shifting it into the future. This can cause misalignments in the assembled point cloud as well as TF errors such as these:

[ERROR] [1673604197.846673112] [rviz2]: Lookup would require extrapolation into the future.  Requested time
1673604197,840177 but the latest data is at time 1673604197,827585, when looking up transform from frame
[laser_data_frame] to frame [map]

This bug may also be the root cause behind #88.

[SteveMacenski]

Ah this is a good point! That would make alot of sense actually!

[DanielBar98]

I tried to edit the code in the conversions.hpp file as follows:

uint64_t shift_time = msg.ranges.size() * msg.time_increment * 1e9 ;  
msg.header.stamp = override_ts == 0 ? t : rclcpp::Time(override_ts- shift_time);

I inserted these lines of code after assigning the msg.ranges and msg.time_increment variables.

The error described by @mintar occurs less often. However, to make the error disappear completely I had to increase the value of the variable time_shift even more. I haven't tried yet by how much exactly (i.e. where the limit is).

I was wondering if the extra amount I have to add up can be explained by the latency of transferring the data (which is balanced out with this) in this timestamp mode (TIME_FROM_ROS_RECEPTION)?

[mintar]

Of course, there is some latency between when the scanner actually recorded the data and when the ROS driver received it. You could try to guess that latency and subtract it here. But you'll only get the static part of the latency that way, not the variable jitter, so the only proper way is to use one of the other timestamp_modes.

However, the TF error should have nothing to do with this latency, because it is a constant offset added to both the published laser scan and the TF transform published by the driver. If you subtract some estimated latency from the laser scan, you also have to subtract it from all the other topics published by this driver.

Perhaps RViz is not properly waiting for the transform to become available?

[SteveMacenski]

Or a small amount of UDP delay getting the data from the sensor if populating with ROS time on reception?

[mintar]

Or a small amount of UDP delay getting the data from the sensor if populating with ROS time on reception?

That delay is there for sure, but as I wrote above: shouldn't that affect both the timestamp of the LaserScan message and the published TF transform, thereby canceling each other out?

[SteveMacenski]

Isn't the laser frame to sensor frame static? Anything from sensor frame -> external frame isn't provided by the sensor.

It should be the same as the PC2 publication though. Perhaps the scan time calculation isn't accurate enough? When you find the time offset that its off by, is it "rounding error" kind of small or "entire laser scan beams" wrong?

It also could be that the PC2 is actually also a little off itself, and that difference isn't actually the laser scan message anymore, but the PC2. We've only analyzed this issue w.r.t. PC2, not ground truth, I don't believe