Go to the documentation of this file.00001
00002
00003 import roslib
00004 roslib.load_manifest( 'rospy' )
00005 import rospy
00006 roslib.load_manifest( 'rosbag' )
00007 import rosbag
00008 import numpy as np
00009 import csv
00010 import sys
00011
00012 def read_file( fname, topic ):
00013 bag = rosbag.Bag( fname )
00014 time = rospy.Time.from_sec(0.0)
00015 msgs = []
00016 for topic, msg, t in bag.read_messages( topics = [topic] ):
00017 if time > t:
00018 print( 'ERROR messages out of order' )
00019 return None
00020 time = t
00021 msgs.append( msg )
00022 bag.close()
00023 return np.asarray( msgs )
00024
00025 arm_joint_names = ['lr_shoulder_pan_joint',
00026 'lr_shoulder_lift_joint',
00027 'lr_upper_arm_roll_joint',
00028 'lr_elbow_flex_joint',
00029 'lr_forearm_roll_joint',
00030 'lr_wrist_flex_joint',
00031 'lr_wrist_roll_joint']
00032
00033
00034 l_arm_joint_names = map(lambda s: s.replace('lr_', 'l_'), arm_joint_names )
00035 r_arm_joint_names = map(lambda s: s.replace('lr_', 'r_'), arm_joint_names )
00036
00037 joint_names = [];
00038
00039 if len( sys.argv ) < 4:
00040 print( 'usage:\n\t rosbagtocsv in.bag out.csv [-r] [-l] [-j joint_name]' );
00041 sys.exit()
00042
00043 in_fname = sys.argv[1];
00044 out_fname = sys.argv[2];
00045
00046 if sys.argv[3] == '-r':
00047 joint_names = r_arm_joint_names
00048 elif sys.argv[3] == '-l':
00049 joint_names = l_arm_joint_names
00050 elif sys.argv[3] == '-j':
00051 joint_names.append( sys.argv[4] )
00052
00053 print( 'searching for %s in %s ...'%(joint_names, in_fname) )
00054
00055 joint_states = read_file( in_fname, '/joint_states' )
00056
00057 if joint_states is None or not len( joint_states ) > 0:
00058 exit
00059
00060 print len( joint_states )
00061 print joint_states[0].name
00062
00063 idx = map( joint_states[0].name.index, joint_names )
00064
00065 writer = csv.writer( open( out_fname, 'wb' ), delimiter = ',', quotechar = '|' )
00066
00067 print( 'writing %d rows to %s ...'%( len( joint_states ), out_fname) )
00068
00069 if len( joint_states ) < 1:
00070 print('bag file doesn not contain joint_states messages.')
00071 sys.exit(-1)
00072
00073 last_time = joint_states[0].header.stamp
00074
00075 for js in joint_states:
00076 pos = map( lambda x: js.position[x], idx )
00077 vel = map( lambda x: js.velocity[x], idx )
00078 effort = map( lambda x: js.effort[x], idx )
00079 time = (js.header.stamp - last_time).to_sec()
00080 row = sum( [[time], pos, vel, effort], [] )
00081 writer.writerow( row )
00082
00083 print( 'done!' )
00084