Sensor data message timestamps should represent data acquisition time

[trey0]

Astrobee FSW generates many sensor data messages with timestamps. For best data quality, it can be important for these timestamps to reflect the sensor data acquisition time as accurately as possible.

For example, you may want to register the location of a HazCam image in a 3D map using localization from bracketing NavCam images, so the best estimate of the HazCam position comes from interpolating between the NavCam positions. But that means any error in the HazCam timestamp will directly affect its estimated position and reduce the quality of the resulting map.

A typical pattern we have been using is to receive data from a sensor driver, do some of our own minor processing to reformat the data as a ROS message, then publish the ROS message with msg.header.stamp set to ros::Time::now(). In other words, the timestamp is set to the publish time rather than the data acquisition time, and these times may differ significantly, with no guarantee that the offset between them is consistent (since the processing time can vary based on system load and other factors).

Since we generally have only one timestamp field available in these messages, and there is an important use case that requires the data acquisition time, I argue we should change our publish code so that msg.header.stamp is set based on the most accurate data acquisition time we can get:

• In the best case, when the data arrives from the sensor, we may be able to propagate an upstream timestamp recorded by the sensor itself, or by the sensor driver. This can eliminate the effect of upstream latency within the sensor, communication, or sensor driver before the data is received by our code.
• If no upstream timestamp is available, we should at least make the call to ros::Time::now() as soon as the data arrives, before we do any further processing.

Furthermore, in some cases, it may make sense to continue propagating the data acquisition time to messages derived from the original sensor data message in downstream nodes. That could be helpful if:

• The original message is not logged, so the downstream message is the only place to find the data acquisition time during post-activity data analysis.
• For some types of data analysis, it may be important to reliably match up multiple messages that were derived from the same sensor frame, in which case it could be very helpful if the messages were guaranteed to have identical timestamps. (Example: the dense mapper needs to process matching point cloud and amplitude_int messages derived from the same HazCam sensor frame.)

Cons of this approach and alternatives:

• Admittedly, it's confusing to use the same msg.header.stamp field with different semantics (publish time vs. data acquisition time) for different messages. However, our semantics are probably not 100% consistent right now, anyway. Some third-party ROS sensor packages use data acquisition timestamp semantics.
• When we have processing pipelines where performance is important, we sometimes use the publish time of messages that correspond to steps in the pipeline as a way to measure the latency of each step. Repurposing the stamp field risks losing some of this latency information.
• Ordering messages by stamp would no longer be guaranteed to preserve the order of when they were actually published, which could make debugging causality confusing in some cases.
• An alternative "best of both worlds" approach would be to add an extra timestamp field in the message that is specifically dedicated to data acquisition time. But changing many message definitions in this way would be a big overhead, probably not worth it.

Summary: I recommend that we start incrementally changing our sensor data messages to use data acquisition timestamp semantics, following the guidelines above.

[rsoussan]

Sounds good to me, I didn't realize the timestamps weren't coming from the sensor acquisition times already. It would be interesting as we do these changes to see how much they differ from the ros::time::now calls currently used.

[marinagmoreira]

What is the status of this after closing the haz cam PR?

[trey0]

What is the status of this after closing the haz cam PR?

The original comment potentially covers timestamps for telemetry from basically all of the hardware. I did a quick audit to see where we could make improvements. Here are the two actions that I think make the cut:

• [x] is_camera nodelet (NavCam/DockCam)

  • Action: Check if the v4l2 driver provides an upstream image timestamp we can use.
  • Timestamp accuracy impact: Could be significant improvement if it factors out the delay for image transfer and/or image processing in the camera driver.
  • Current timestamp source

• [x] sci_cam_image APK (SciCam)

  • Action: Check if the Android imaging interface provides an upstream timestamp from the camera driver.
  • Timestamp accuracy impact: Could be significant improvement if it factors out the delay for image transfer and/or image processing in the camera driver.
  • Does the Image.getTimestamp() method do anything useful? Maybe @kbrowne15 already found it didn't work?
  • Current timestamp source

[trey0]

And below are some other possible components I evaluated but don't think are important to work on right now.

• pico_driver (HazCam/PerchCam)
  • DONE - #413
• epson_imu
  • Action: From what I can tell, the best time to record the timestamp would be right after we get the GPIO interrupt indicating a new IMU sample is available. Perhaps at the beginning of the BurstRead() function would be convenient. There does not appear to be an upstream timestamp available from the IMU itself.
  • Timestamp accuracy impact: Probably minor, dominated by the delay in serial comms with the IMU.
  • Current timestamp source
• perching_arm
  • Action: From what I can tell, the best time to record the timestamp would be right after the first byte of a new telemetry message is received frome the arm, i.e., where the FSM advances to PROTOCOL_WF_HEADER2.
  • Timestamp accuracy impact: Probably minor, dominated by the delay in serial comms with the arm. There could be a broader follow-up discussion with Michael Dille and folks like Astrobatics/Gecko Gripper about how to improve the usefulness of arm telemetry. I suspect the low arm telemetry sampling rate is a bigger issue in practice than this timestamp delta. Maybe some payloads already made relevant improvements to the arm firmware on various branches?
  • Current timestamp source
• pmc_actuator
  • Action: From what I can tell, the best time to record the timestamp would be right after the Read() call in GetTelemetry() succeeds.
  • Timestamp accuracy impact: Probably very minor. This change unfortunately can't eliminate the serial comm delay, due to the way the comm is implemented.
  • Current timestamp source

[kbrowne15]

In regards to the SciCam question, when querying the scicam, the timestamp source is set to unknown. According to android documentation, this means "Timestamps from [android.sensor.timestamp] are in nanoseconds and monotonic, but can not be compared to timestamps from other subsystems (e.g. accelerometer, gyro etc.), or other instances of the same or different camera devices in the same system with accuracy." Because of this, we decided not to use the getTimestamp() method.

Instead, we get the time for the image in the capture complete callback. This time is before the image gets packaged up by the camera, returned to the apk, and then processed by the apk so it should be the time closest to when the image was captured.

[trey0]

... Because of this, we decided not to use the getTimestamp() method.

Thanks @kbrowne15.

Sorry if we previously discussed this, but did you try to evaluate the getTimestamp() quality? It still seems possible that the accuracy is better even if the driver developers chose not to vouch for it by setting the source information. Have you already compared it to the capture timestamp and have feedback about the quality?

It might be worth logging the getTimestamp() result side-by-side with the capture timestamp (e.g., in a rosout message), so anybody doing post-processing on the ground could compare them and potentially try out using the upstream timestamp for mapping. Does that sound like something we could try logging without too much trouble?

[kbrowne15]

@trey0 Sorry, I have should have copied more of the description for the unknown timestamp source. Please see timestamp source unknown for more info. Basically, when the timestamp source is unknown, getTimestamp() returns the milliseconds since boot time of the device, in this case the HLP. So just getting the value and outputting it is not going to be very helpful. Brian and I briefly talked about trying to get the boot time of the HLP, somehow adjusting for when the set time script is used, and then adding that to what getTimestamp() returns but we agreed that sounded like a lot of work for a timestamp that may or may not be accurate. Also, I worry that we could introduce error if we aren't able to capture the boot time correctly.

[trey0]

Aha! Based on that reference, we should try:

long bestTimestampMillis = ((new Date()).getTime() - SystemClock.uptimeMillis()) + image.getTimestamp();

[kbrowne15]

Okay, I'll put that in the adb logcat for now. Oleg is very familiar with the logcat and will know how to extract this information. If he doesn't end up being the one to look at it, it shouldn't be hard for someone else to extract it.

[bcoltin]

Should be able to use timestamp from v4l2 buffer https://01.org/linuxgraphics/gfx-docs/drm/media/uapi/v4l/buffer.html#c.v4l2_buffer in is_camera driver. Currently uses ros time now which has a delay.

[trey0]

Should be able to use timestamp from v4l2 buffer https://01.org/linuxgraphics/gfx-docs/drm/media/uapi/v4l/buffer.html#c.v4l2_buffer in is_camera driver. Currently uses ros time now which has a delay.

Cool! Unfortunately the URL now gives a 404. Here is another [1]. TIS also confirms their v4l2 driver provides "monotonic time" (elapsed time since boot) [2]. To convert that to UTC time, I think you need to basically add gettimeofday() - clock_gettime(). Just like the discussion above with the SciCam.

[1] https://www.kernel.org/doc/html/v4.9/media/uapi/v4l/buffer.html (look at description of timestamp field) [1] https://www.theimagingsource.com/documentation/tiscamera/timestamps.html