Hal T Hal T - 1 year ago 78
Python Question

(Python) Converting image time series to ROS bag

I have a set of images with metadata including time stamps and odometry data from a data set. I want to convert this time series of images into a rosbag so I can easily plug it out of the code I'll write and switch in real time data after testing. By the tutorial, the process of programmatically saving data to a rosbag seems to work like:

import rosbag
from std_msgs.msg import Int32, String

bag = rosbag.Bag('test.bag', 'w')

str = String()
str.data = 'foo'

i = Int32()
i.data = 42

bag.write('chatter', str)
bag.write('numbers', i)

For images, the data type is different but the process is the same. However, when I have data that is supposed to be associated with each other, e.g. each image should be paired with a timestamp and an odometry recording. This sample code seems to write "chatter" and "numbers" disjointly. What would be the way to publish data so that for each image, it would be paired with other pieces of data automatically?

In other words, I need help with the commented lines in the following code:

import rosbag
import cv2
from std_msgs.msg import Int32, String
from sensor_msgs.msg import Image

bag = rosbag.Bag('test.bag', 'w')
for data in data_list:
(imname, timestamp, speed) = data
ts_str = String()
ts_str.data = timestamp

s = Float32()
s.data = speed

image = cv2.imread(imname)

# convert cv2 image to rosbag format
# write image, ts_str and s to the rosbag jointly


Answer Source

Many of the standard message types like sensor_msgs/Image or nav_msgs/Odometry have an attribute header which contains an attribute stamp which is meant to be used for timestamps.

When creating the bag file, simply set the same timestamp in the headers of messages that belong together and then write them to separate topics:

from sensor_msgs.msg import Image
from nav_msgs.msg import Odometry


img_msg = Image()
odo_msg = Odometry()
img_msg.header.stamp = timestamp
odo_msg.header.stamp = timestamp

# set other values of messages...

bag.write("image", img_msg)
bag.write("odometry", odo_msg)

Later when subscribing for the messages, you can then match messages from the different topics based on their timestamp.

Depending on how important exact synchronization of image and odometry is for you application, it might be enough to just use the last message you got on each topic and assume they fit together. In case you need something more precise, there is a message_filter which takes care of synchronizing two topics and provides the messages of both topics together in one callback.