Export image from rosbag

rosbag_image_export.py

import rospy
import rosbag
import numpy as np
import cv2
import message_filters
import sys, os
from sensor_msgs import point_cloud2
import glob
from cv_bridge import CvBridge, CvBridgeError

def get_sync_messages(bagfile, topics):
  synced_msgs = [[] for t in topics]
  # callback function for storing the synced messages
  def sync_messages(*msg_arg):
    for i, msg in enumerate(msg_arg):
      #print msg
      synced_msgs[i].append(msg)

  fs = [message_filters.SimpleFilter() for topic in topics]
  N = 60000
  ts = message_filters.ApproximateTimeSynchronizer(fs, N, 10000000000000000)
  ts.registerCallback(sync_messages)
  # read the bag messages
  print('Start read the bag messages')

  bag = rosbag.Bag(bagfile)
  print('read the bag messages done')

  all_topics = bag.get_type_and_topic_info()[1]
  print('Has topics:\n{}\n'.format(all_topics))

  msg_iter = bag.read_messages(topics=topics)
  for topic, msg, stamp in msg_iter:
    fs[topics.index(topic)].signalMessage(msg)
  print('sync done')
  return synced_msgs

if __name__ == '__main__':


  bag_dir = sys.argv[1]
  out_dir = sys.argv[2]
  topics = [
  '/sensor/11/image_all/compressed', 
  '/sensor/22/image_all/compressed',
  '/sensor/33/image_all/compressed',
  '/sensor/44/image_all/compressed']


  bags_path = glob.glob(os.path.join(bag_dir,'*.bag'))
  print('Find:\n{}\n'.format(bags_path))
  for bag_path in bags_path:
    print('Sync:\n{}\n'.format(bag_path))
    synced_msgs = get_sync_messages(bag_path,topics)
    print('sync done, will start write file')
    print [len(m) for m in synced_msgs]

    bagname = os.path.basename(bag_path).split('.')[0]
    out_dir=os.path.join(out_dir,bagname)
    if not os.path.exists(out_dir):
      os.makedirs(out_dir)
    n_msgs = len(synced_msgs[0])
    print('n_msgs={}'.format(n_msgs))

    for i in xrange(n_msgs):
      #get front back left right image
      for j in range(4):
        im_msg = synced_msgs[j][i]
        #### direct conversion to CV2 ####
        np_arr = np.fromstring(im_msg.data, np.uint8)
        im = cv2.imdecode(np_arr,cv2.IMREAD_COLOR)
        # cv2.imshow('cv_img', im)
        # cv2.waitKey(1)
        out_imfile = '%s/%s_%08d_%d.png' %(out_dir, bagname, i,j)
        cv2.imwrite(out_imfile, im)
        print('write file:{}'.format(out_imfile))