How to loop over each timestep in rosbags using Python?

Question

I am using rosbag api to record rgb, depth and imu data coming off a robot. For an offline Visual Odometry experiment, I am in need of synchronizing RGB data with the imu data at the correct timesteps. This is because the IMU in our robot updated much faster than the RGB camera thus we have over 1000 entries for IMU data whereas we have only 136 images recorded within the same duration.

These are the information on the rosbag collected

1. Number of topics - 3 /rgb_image, /depth, imu
2. Time - 20s

I plan on iterating over each timestep and extracting the IMU data that is closest to the chosen picture's timestep. The end goal is to create a new rosbag with the synchronized data.

My question is how do I iterate over each image in the correct time order from the rosbag file using Python?

Answer 1

The rosbag code API page includes an example of how to use the Python API to read messages out of a bag. It's a simple 5-line program.

You need to replace line 4 (the print statement) with your own code that iterates over all messages in the bag, pulling out the RGB messages and writing them to the new bag while also pulling out the IMU messages and finding the one closest to each RGB message.

Something like this should get you started. Note that this code uses the most recent IMU message, which is not necessarily the closest in time if the one that comes after the RGB message is closer.

import rosbag

input_bag = rosbag.Bag('input.bag')
output_bag = rosbag.Bag('output.bag', 'w')

most_recent_imu = None
for topic, msg, t in input_bag.read_messages(topics=['rgb_image', 'depth', 'imu']):
    # Store the most recent IMU message
    if topic == 'imu':
        most_recent_imu = msg
    elif topic == 'rgb_image':
        output_bag.write('rgb_image', msg)
        output_bag.write('imu', most_recent_imu)

output_bag.close()
input_bag.close()