Discrepancy in Timestamps of IMU Data from OAK-D Pro Wide and IMU frequency

[jodusan]

Describe the bug Hello,

I am currently working with the OAK-D Pro Wide camera, utilizing the depthai-ros package to read IMU data and save it to an MCAP file. However, I've encountered an issue with the timestamps of the IMU data.

1) There seems to be a significant discrepancy between the timestamps in the ROS message header and the actual time of reading as indicated by Python's time.time(). This difference is causing synchronization issues in my data processing pipeline since messages appear to arrive in batches.

2) I am unable to get above 150~160hz for imu messages which should go to 400 if I am not mistaken.

Expected behavior I was expecting to see more uniform arrival of imu messages that more closely correlates with header timestamp

Screenshots Orange line represents ros message timestamps, blue line represents actual python time.time of the messages

Am I missing something? I cannot get orbslam3 to work with files that are played from rosbag files

[Serafadam]

Hi, do you encounter those issues only when playing data from a rosbag, or also when running with a real camera? ROS Timestamps might differ a bit from the ones reported via chrono/python time depending on the setup. Regarding IMU frequency, depending on the pipeline set and the amount of data transferred from the device the IMU frequency might need to be tuned with batch report parameters, changing camera parameters (setISP3A for example)

[jodusan]

@Serafadam

The issue appears both with a recorded ros bag and a live camera feed. My camera parameters are:

/oak:
  ros__parameters:
    camera:
      i_nn_type: none
      i_pipeline_type: RGBD
      i_enable_ir: true
      i_enable_imu: true
      i_laser_dot_brightness: 0
#      i_publish_tf_from_calibration: true
#      i_tf_imu_from_descr: true
    right:
      i_publish_topic: true
      i_resolution: "400P"
#      r_set_man_exposure: true
    left:
      i_publish_topic: true
      i_resolution: "400P"
#      r_set_man_exposure: true
    imu:
      i_acc_freq: 500
      i_gyro_freq: 400
      i_sync_method: COPY
      i_batch_report_threshold: 1
      i_max_batch_reports: 10

Here is the script used to generate the plot

import time
import matplotlib.pyplot as plt
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu, Image
topics = [['/imu', '/camera/left'], ['/oak/imu/data', '/oak/left/image_raw']]
class TimestampCollector(Node):
    def __init__(self):
        super().__init__('timestamp_collector')
        self.subscription = self.create_subscription(
            Imu,
            topics[1][0],  # Replace with your topic name
            self.listener_callback,
            1)
        self.subscription2 = self.create_subscription(
            Image,
            topics[1][1],  # Replace with your topic name
            self.listener_callback_image,
            1)
        self.timestamps = []
        self.timestamps_ros = []
        self.timestamps_img = []
        self.timestamps_ros_img = []
        self.message_count = 0
        self.message_count_imu = 0
    def listener_callback(self, msg):
        # print(msg)
        self.timestamps.append(time.time())
        self.timestamps_ros.append(msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9)
        self.message_count_imu += 1
    def listener_callback_image(self, msg):
        self.timestamps_img.append(time.time())
        self.timestamps_ros_img.append(msg.header.stamp.sec + msg.header.stamp.nanosec / 1e9)
        self.message_count += 1
        if self.message_count >= 100:
            self.plot_timestamps()
            exit()
    def plot_timestamps(self):
        plt.plot(self.timestamps, [1] * len(self.timestamps), marker='o')
        plt.plot(self.timestamps_ros, [2] * len(self.timestamps_ros), marker='o')
        plt.plot(self.timestamps_img, [3] * len(self.timestamps_img), marker='o')
        plt.plot(self.timestamps_ros_img, [4] * len(self.timestamps_ros_img), marker='o')
        plt.ylabel('Message Number')
        plt.xlabel('Timestamp')
        plt.title('Timestamps of Received Messages')
        plt.show()
def main(args=None):
    rclpy.init(args=args)
    timestamp_collector = TimestampCollector()
    rclpy.spin(timestamp_collector)
    timestamp_collector.destroy_node()
    rclpy.shutdown()
if __name__ == '__main__':
    main()

Could you be a bit more specific about what exactly should I do with setISP3A example? What can i change in python bindings?

[Serafadam]

Could you show the header stamp plots from plotjuggler? I think this may be the best plotting option here. Regarding isp3a, you can set it using parameters, for example:

 rgb:       
      i_set_isp3a_fps: true
      i_isp3a_fps: 10