I use the Python script below as a ROS node to convert a bag file to a set of images in a directory. To run I use:
rosrun my_package bag_to_images.py ../saved_images ../input.bag
where the saved_images/
directory must exist beforehand. Also, give the location of saved_images/
relative to the directory where the bag_to_images.py script is located. The input.bag
file should also be specified relative to the directory where the bag_to_images.py script is located. It is possible to use this in a launch file but I never do that.
For each image topic just put another if topic ==
section into the script and modify the topic names to match those in your bag file. The images get saved to saved_images/right_{message timestamp}.pgm
. To save as .png or .jpg just change those lines to use those file extensions, as OpenCV is smart enough to figure out how to write the files the correct way. See here at the cv::imread section for supported formats with OpenCV.
#!/usr/bin/python
# Start up ROS pieces.
PKG = 'my_package'
import roslib; roslib.load_manifest(PKG)
import rosbag
import rospy
import cv
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
# Reading bag filename from command line or roslaunch parameter.
import os
import sys
class ImageCreator():
# Must have __init__(self) function for a class, similar to a C++ class constructor.
def __init__(self):
# Get parameters when starting node from a launch file.
if len(sys.argv) < 1:
save_dir = rospy.get_param('save_dir')
filename = rospy.get_param('filename')
rospy.loginfo("Bag filename = %s", filename)
# Get parameters as arguments to 'rosrun my_package bag_to_images.py <save_dir> <filename>', where save_dir and filename exist relative to this executable file.
else:
save_dir = os.path.join(sys.path[0], sys.argv[1])
filename = os.path.join(sys.path[0], sys.argv[2])
rospy.loginfo("Bag filename = %s", filename)
# Use a CvBridge to convert ROS images to OpenCV images so they can be saved.
self.bridge = CvBridge()
# Open bag file.
with rosbag.Bag(filename, 'r') as bag:
for topic, msg, t in bag.read_messages():
if topic == "/left/image_raw":
try:
cv_image = self.bridge.imgmsg_to_cv(msg, "bgr8")
except CvBridgeError, e:
print(e)
timestr = "%.6f" % msg.header.stamp.to_sec()
image_name = str(save_dir)+"/left_"+timestr+".pgm"
#image_name = str(save_dir)+"/"+timestr+"_left"+".pgm"
cv.SaveImage(image_name, cv_image)
if topic == "/right/image_raw":
try:
cv_image = self.bridge.imgmsg_to_cv(msg, "bgr8")
except CvBridgeError, e:
print(e)
timestr = "%.6f" % msg.header.stamp.to_sec()
image_name = str(save_dir)+"/right_"+timestr+".pgm"
#image_name = str(save_dir)+"/"+timestr+"_right"+".pgm"
cv.SaveImage(image_name, cv_image)
# Main function.
if __name__ == '__main__':
# Initialize the node and name it.
rospy.init_node(PKG)
# Go to class functions that do all the heavy lifting. Do error checking.
try:
image_creator = ImageCreator()
except rospy.ROSInterruptException: pass