00001
00002
00003 PKG = 'stereo_msgs'
00004 import roslib; roslib.load_manifest(PKG)
00005 import rospy
00006 import rosrecord
00007 import fileinput
00008 import os
00009
00010 def split_rawstereo(inbags):
00011 for b in inbags:
00012 print "Trying to migrating file: %s"%b
00013 outbag = b+'.tmp'
00014 rebag = rosrecord.Rebagger(outbag)
00015 try:
00016 for (topic, msg, t) in rosrecord.logplayer(b, raw=False):
00017 if topic == 'stereo/raw_stereo' or topic == '/stereo/raw_stereo':
00018 m = msg.left_image
00019 m.header.frame_id = msg.header.frame_id
00020 m.header.stamp = msg.header.stamp
00021 rebag.add('stereo/left/image_raw', m, t, raw=False)
00022 m = msg.right_image
00023 m.header.frame_id = msg.header.frame_id
00024 m.header.stamp = msg.header.stamp
00025 rebag.add('stereo/right/image_raw', m, t, raw=False)
00026 m = msg.left_info
00027 m.header.frame_id = msg.header.frame_id
00028 m.header.stamp = msg.header.stamp
00029 rebag.add('stereo/left/camera_info', m, t, raw=False)
00030 m = msg.right_info
00031 m.header.frame_id = msg.header.frame_id
00032 m.header.stamp = msg.header.stamp
00033 rebag.add('stereo/right/camera_info', m, t, raw=False)
00034 else:
00035 rebag.add(topic, msg, t, raw=False)
00036 rebag.close()
00037 except rosrecord.ROSRecordException, e:
00038 print " Migration failed: %s"%(e.message)
00039 os.remove(outbag)
00040 continue
00041
00042 oldnamebase = b+'.old'
00043 oldname = oldnamebase
00044 i = 1
00045 while os.path.isfile(oldname):
00046 i=i+1
00047 oldname = oldnamebase + str(i)
00048 os.rename(b, oldname)
00049 os.rename(outbag, b)
00050 print " Migration successful. Original stored as: %s"%oldname
00051
00052 if __name__ == '__main__':
00053 import sys
00054 if len(sys.argv) >= 2:
00055 split_rawstereo(sys.argv[1:])
00056 else:
00057 print "usage: split_rawstereo.py bag1 [bag2 bag3 ...]"
00058
00059
