rosbag Python API not reading messages
Hi
I'm trying to read several topics using the rosbag
Python API. However, I'm getting no message back at all. I can call rosbag.get_start_time()
and rosbag.get_end_time()
, and they give the correct values. Here's my code:
from __future__ import print_function
import rosbag
import tf2_ros
import genpy
import os
class RGBDTester:
def __init__(self):
# input/output paths
bag_file_path = 'test2.bag'
# image topics with CompressedImage
self.topic_rgb = '/camera/rgb/image_raw'
self.topic_depth = '/camera/depth_registered/image_raw'
# camera intrinsics with CameraInfo
self.topic_ci = '/camera/rgb/camera_info'
self.topics_tf = ["/tf", "/tf_static"]
self.topics = [self.topic_rgb, self.topic_depth, self.topic_ci]
bag_file_path = os.path.expanduser(bag_file_path)
print("reading:", bag_file_path)
self.bag = rosbag.Bag(bag_file_path, mode='r')
print("duration:", self.bag.get_end_time()-self.bag.get_start_time(),"s")
ref_times = []
# fill the tf buffer
# maximum duration to cache all transformations
# https://github.com/ros/geometry2/issues/356
time_max = genpy.Duration(secs=pow(2, 31) - 1)
tf_buffer = tf2_ros.Buffer(cache_time=time_max)
for topic, msg, t in self.bag.read_messages(topics=self.topics_tf):
for transf in msg.transforms:
tf_buffer.set_transform(transf, "tester")
topic_times = dict()
for t in self.topics:
topic_times[t] = []
full_jnt_values = dict() # store first (oldest) joint values of complete set
for topic, msg, t in self.bag.read_messages(topics=self.topics):
topic_times[topic].append(msg.header.stamp)
if len(topic_times[self.topic_rgb])==0:
print("NO colour images. Check that topic '"+self.topic_rgb+"' is present in bag file!")
if len(topic_times[self.topic_depth]) == 0:
print("NO depth images. Check that topic '"+self.topic_depth+"' is present in bag file!")
# remove topics with no messages
[topic_times.pop(top, None) for top in topic_times.keys() if len(topic_times[top]) == 0]
if not topic_times:
bag_topics = self.bag.get_type_and_topic_info().topics
print("Found no messages on any of the given topics.")
print("Valid topics are:", bag_topics.keys())
print("Given topics are:", self.topics)
camera_info = None
for topic, msg, t in self.bag.read_messages(topics=[self.topic_ci]):
camera_info = msg
if camera_info is None:
raise Exception("No CameraInfo message received!")
def __del__(self):
# close log file
if hasattr(self, 'bag'):
self.bag.close()
if __name__ == '__main__':
RGBDTester()
The output I get is the following:
reading: test2.bag
duration: 4.97503685951 s
NO colour images. Check that topic '/camera/rgb/image_raw' is present in bag file!
NO depth images. Check that topic '/camera/depth_registered/image_raw' is present in bag file!
Found no messages on any of the given topics.
Valid topics are: ['camera/depth_registered/camera_info', 'camera/rgb/image_raw', 'camera/depth_registered/image_raw', 'tf_static', 'camera/rgb/camera_info']
Given topics are: ['/camera/rgb/image_raw', '/camera/depth_registered/image_raw', '/camera/rgb/camera_info']
Traceback (most recent call last):
File "/home/mwell/catkin_ws/src/rgbd_export/src/test.py", line 81, in <module>
RGBDTester()
File "/home/mwell/catkin_ws/src/rgbd_export/src/test.py", line 71, in __init__
raise Exception("No CameraInfo message received!")
Exception: No CameraInfo message received!
As you can see, no message is received at all. Here's the rosbag file. Anyone know why this is happening?
Thanks