Go to the documentation of this file.00001
00002 import roslib
00003 roslib.load_manifest("biotac_log_parser")
00004 import rospy
00005 import rosbag
00006 import numpy
00007 import glob
00008 import sys
00009 import cPickle
00010 from optparse import OptionParser
00011 import itertools
00012 import h5py
00013 from biotac_sensors.msg import BioTacHand
00014
00015 from collections import defaultdict
00016
00017 def main():
00018 """ Usage
00019 First log the data using rosbag:
00020 rosbag record /biotac_pub [other topics] -o file_prefix
00021
00022 Many topics can be recorded, but this file only parses biotac msgs.
00023 The call rosrun rosrun biotac_log_parser parse_log_json.py -i bag_file -o output.hdf5
00024
00025 Multiple bag files can be specified on the command line using wildcards, but they need to be
00026 enclosed in quotes:
00027 The call rosrun rosrun biotac_log_parser parse_log_json.py -i "*.bag" -o output.hdf5
00028 """
00029
00030 parser = OptionParser()
00031 parser.add_option("-i", "--input_file", dest="input_file",
00032 help="input bag FILEs. Use wildcards for multiple files",
00033 metavar="FILE", action="store")
00034 parser.add_option("-o", "--output_file", dest="output_file",
00035 help="output FILE", metavar="FILE")
00036
00037 (options, _) = parser.parse_args()
00038
00039 if options.input_file is None:
00040 rospy.logerr("The input bag has to be specified")
00041 sys.exit()
00042 if options.output_file is None:
00043 rospy.logerr("The output file has to be specified")
00044 sys.exit()
00045
00046
00047 f = h5py.File(options.output_file, "w")
00048
00049 gen = (glob.glob(f) for f in options.input_file.split())
00050 traj_num = 0
00051
00052 for filename in itertools.chain.from_iterable(gen):
00053
00054 rospy.loginfo("Opening bag %s"% filename)
00055 bag = rosbag.Bag(filename)
00056
00057 tdc_data = defaultdict(list)
00058 tac_data = defaultdict(list)
00059 pdc_data = defaultdict(list)
00060 pac_data = defaultdict(list)
00061 electrode_data = defaultdict(list)
00062 time_stamp = []
00063
00064 num_fingers = 0
00065 for _, msg, _ in bag.read_messages(topics="/biotac_pub"):
00066 isinstance(msg, BioTacHand)
00067 tdc = []
00068 num_fingers = len(msg.bt_data)
00069 for finger_index in xrange(num_fingers):
00070 tdc.append( finger.tdc_data)
00071 tac.append( finger.tac_data)
00072 pdc.append( finger.pdc_data)
00073 pac.append( finger.pac_data)
00074 electrode.append( finger.electrode_data)
00075
00076 tdc_data[finger_index].append( msg.bt_data[finger_index].tdc_data)
00077 tac_data[finger_index].append( msg.bt_data[finger_index].tac_data)
00078 pdc_data[finger_index].append( msg.bt_data[finger_index].pdc_data)
00079 pac_data[finger_index].append( msg.bt_data[finger_index].pac_data)
00080 electrode_data[finger_index].append( msg.bt_data[finger_index].electrode_data)
00081
00082 time_stamp.append( msg.header.stamp.to_sec())
00083
00084
00085
00086 group_name = filename
00087 f[group_name + "/timestamps"] = time_stamp
00088 for finger_index in xrange(num_fingers):
00089 finger_name = "/finger_" + str(finger_index)
00090 f[group_name + finger_name + "/tdc_data"] = tdc_data[finger_index]
00091 f[group_name + finger_name + "/tac_data"] = tac_data[finger_index]
00092 f[group_name + finger_name + "/pdc_data"] = pdc_data[finger_index]
00093 f[group_name + finger_name + "/pac_data"] = pac_data[finger_index]
00094 f[group_name + finger_name + "/electrode_data"] = electrode_data[finger_index]
00095
00096 traj_num += 1
00097 f.close()
00098
00099
00100 if __name__ == "__main__":
00101 main()