rosbagファイルからjpegを抽出する方法

by zienchan

目的

分割されたrosbagファイルから、対象のセンサノード(ここでは受光センサ)の全フレームをjpegとして出力する
なおファイル名はフレーム番号の他、受信時間(ナノ秒)も付与する

環境

方法

  1. UbuntuにROS Medlodicをインストールする
  2. 下記のようなpythonコードを用意する
#! /usr/bin/python

import os
import sys
import cv2
import csv
import rospy, rosbag
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

if len(sys.argv) == 3:
   sys.argv[1] = sys.argv[1] + ".dir"
   out_dir = sys.argv[1]
   if not os.path.exists(sys.argv[1]):
       os.makedirs(out_dir)

   else:
       print sys.argv[1], 'already exists'
       sys.exit()

   bagfile = sys.argv[2]

else:
   print 'illegal arguments exeption.'
   sys.exit()

bridge = CvBridge()

def main():

   topicNames = {'/front_camera/image_color/compressed'
               , '/rear_camera/image_color/compressed'}

   inbag_name = bagfile

   print "open rosbag file..."
   bag = rosbag.Bag(inbag_name)
   for topic, msg, t in bag.read_messages():
       if topic in topicNames:
           try:
               img = bridge.compressed_imgmsg_to_cv2(msg)
           except CvBridgeError, e:
               print(e)
           else:
               filename = '/' + str(msg.header.seq) + '_' + str(msg.header.stamp) + '_' + topic.split('/')[1].replace('_', '-') + '.jpg'
               cv2.imwrite(sys.argv[1] + filename, img)
               with open(sys.argv[1]+'/metadata.csv','ab') as f:
                   thewriter = csv.writer(f)
                   thewriter.writerow([str(msg.header.seq),str(msg.header.stamp),os.getcwd()+'/'+sys.argv[1]+'/', filename])

   bag.close()

if __name__ == "__main__":
   main()

PythonでなくともC++でも可能だが環境を作るのが面倒だったのでここではpythonを使用している
またROSはpython2のみサポートしているようだ
他の方法としてROS Medlodicに付属してくるrosbagの標準コマンドを使用する方法もあるがリアルタイムシミュレートのためか環境によってはフレームが欠落するので今回は見送った
またこいつは連番しか付与できないのでタイムスタンプが出力できないことにも注意
3. 下記のようなコマンドで一括で実行する

find -name "*.rosbag" | xargs -I@ sh -c "python2 ~/tmp/__main__.py @ @"

結果

全フレームがjpegで出力される
ただし複数カメラなどで、あるカメラを基準に最も近いほかカメラのフレームを取得したい場合(フレームを同期したい)にはこれでは不十分である
それを解決したスクリプトもあるのだが、需要があればまた今後どこかでまとめたい