wrong image timestamps under heavy load

[christian-rauch]

Describe the bug
We encountered that images are published with wrong timestamps when the CPU or GPU is under heavy load. When this happens, the driver prints the warning Initializing or re-initializing the device to realtime offset.

To Reproduce
Steps to reproduce the behaviour:

1. start driver
2. stress the PC with CPU and/or GPU load
3. log colour images into bag file
4. play back the log file and subscribe to the colour images
5. when there is motion in the scene (e.g. from left to right) you should see "jumps" while the timestamps are monotonic continuous

Expected behaviour
As far as I understand, the camera has a dedicated hardware clock. I would expect that, while images may be dropped under heavy load, the timestamps and image data stays synchronised and that the timestamps increase monotonically.

Desktop (please complete the following information):

• OS: Ubuntu
• Version 20.04

Additional context

The function in question is K4AROSDevice::updateTimestampOffset:

Azure_Kinect_ROS_Driver/src/k4a_ros_device.cpp

Lines 1492 to 1525 in c0742b9

	void K4AROSDevice::updateTimestampOffset(const std::chrono::microseconds& k4a_device_timestamp_us,
	const std::chrono::nanoseconds& k4a_system_timestamp_ns)
	{
	// System timestamp is on monotonic system clock.
	// Device time is on AKDK hardware clock.
	// We want to continuously estimate diff between realtime and AKDK hardware clock as low-pass offset.
	// This consists of two parts: device to monotonic, and monotonic to realtime.
	// First figure out realtime to monotonic offset. This will change to keep updating it.
	std::chrono::nanoseconds realtime_clock = std::chrono::system_clock::now().time_since_epoch();
	std::chrono::nanoseconds monotonic_clock = std::chrono::steady_clock::now().time_since_epoch();
	std::chrono::nanoseconds monotonic_to_realtime = realtime_clock - monotonic_clock;
	// Next figure out the other part (combined).
	std::chrono::nanoseconds device_to_realtime =
	k4a_system_timestamp_ns - k4a_device_timestamp_us + monotonic_to_realtime;
	// If we're over a second off, just snap into place.
	if (device_to_realtime_offset_.count() == 0 ||
	std::abs((device_to_realtime_offset_ - device_to_realtime).count()) > 1e7)
	{
	ROS_WARN_STREAM("Initializing or re-initializing the device to realtime offset: " << device_to_realtime.count()
	<< " ns");
	device_to_realtime_offset_ = device_to_realtime;
	}
	else
	{
	// Low-pass filter!
	constexpr double alpha = 0.10;
	device_to_realtime_offset_ = device_to_realtime_offset_ +
	std::chrono::nanoseconds(static_cast<int64_t>(
	std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count())));
	}
	}

which resets the time offset and thus causes that the image data does not have a monotonic time anymore.

Is there documentation on this behaviour? The sensor has a separate hardware clock that should make sure that image data always has a monotonic time source. From my understanding, the system and the sensor both have monotonic clocks, but with different offsets. The device_to_realtime_offset_ (the offset between the world time on the system and the local time on the sensor) should only be set once when the sensor starts, and never change. What is the purpose of resetting this offset?