validate timestamps

validate_timestamps.py

import numpy as np
import rosbag

input_bag_path = "/home/goretkin/repos/Hg_Scene_Extractor/bags/ekffff.bag"

bag_ts = []
odoms = []
with rosbag.Bag(input_bag_path) as bag:
    # promiscuously grab all possible odometry topics. Assumes that bags contain only one of each.
    for topic, msg, t in bag.read_messages(topics=["/aft_mapped_to_init", "/odometry/filtered"]):
        bag_ts.append(t)
        odoms.append(msg)

    for topic, msg, t in bag.read_messages(topics=["/tracked_objects"]):
        bag_ts.append(t)
        odoms.append(msg.ego)


odom_headers = np.array([m.header for m in odoms])
odom_ts = np.array([h.stamp.to_sec() for h in odom_headers])
odom_ts_rel = odom_ts - odom_ts[0]

delta_odom_ts = np.diff(odom_ts)

if np.any(delta_odom_ts == 0):
    print("repeated time stamps")


if np.any(delta_odom_ts < 0):
    print("non-monotonic time stamps")