00001
00002
00003 import sys
00004 import numpy as np
00005 import scipy.io
00006
00007 import roslib
00008 roslib.load_manifest('hrl_phri_2011')
00009 import rospy
00010
00011 import rosbag
00012
00013 def main():
00014
00015 field_map = {
00016 "/l_cart/f_cmd" :
00017 {
00018 "wrench.force.x" : "f_cmd_x",
00019 "wrench.force.y" : "f_cmd_y",
00020 "wrench.force.z" : "f_cmd_z",
00021 "wrench.torque.x" : "t_cmd_x",
00022 "wrench.torque.y" : "t_cmd_y",
00023 "wrench.torque.z" : "t_cmd_z"
00024 },
00025 "/l_cart/f_err" :
00026 {
00027 "wrench.force.x" : "f_err_x",
00028 "wrench.force.y" : "f_err_y",
00029 "wrench.force.z" : "f_err_z",
00030 "wrench.torque.x" : "t_err_x",
00031 "wrench.torque.y" : "t_err_y",
00032 "wrench.torque.z" : "t_err_z"
00033 },
00034 "/l_cart/k_effective" :
00035 {
00036 "wrench.force.x" : "k_eff_x",
00037 "wrench.force.y" : "k_eff_y",
00038 "wrench.force.z" : "k_eff_z",
00039 "wrench.torque.x" : "kt_eff_x",
00040 "wrench.torque.y" : "kt_eff_y",
00041 "wrench.torque.z" : "kt_eff_z"
00042 },
00043 "/l_cart/sensor_ft" :
00044 {
00045 "wrench.force.x" : "f_sens_x",
00046 "wrench.force.y" : "f_sens_y",
00047 "wrench.force.z" : "f_sens_z",
00048 "wrench.torque.x" : "t_sens_x",
00049 "wrench.torque.y" : "t_sens_y",
00050 "wrench.torque.z" : "t_sens_z"
00051 },
00052 "/l_cart/sensor_raw_ft" :
00053 {
00054 "wrench.force.x" : "f_sens_raw_x",
00055 "wrench.force.y" : "f_sens_raw_y",
00056 "wrench.force.z" : "f_sens_raw_z",
00057 "wrench.torque.x" : "t_sens_raw_x",
00058 "wrench.torque.y" : "t_sens_raw_y",
00059 "wrench.torque.z" : "t_sens_raw_z"
00060 },
00061 "/l_cart/state/x" :
00062 {
00063 "pose.position.x" : "pos_x",
00064 "pose.position.y" : "pos_y",
00065 "pose.position.z" : "pos_z",
00066 "pose.orientation.x" : "rot_x",
00067 "pose.orientation.y" : "rot_y",
00068 "pose.orientation.z" : "rot_z",
00069 "pose.orientation.w" : "rot_w",
00070 },
00071 "/l_cart/state/xd" :
00072 {
00073 "linear.x" : "vel_x",
00074 "linear.y" : "vel_y",
00075 "linear.z" : "vel_z",
00076 "angular.x" : "avel_x",
00077 "angular.y" : "avel_y",
00078 "angular.z" : "avel_z"
00079 },
00080 "/l_cart/x_err" :
00081 {
00082 "linear.x" : "pos_err_x",
00083 "linear.y" : "pos_err_y",
00084 "linear.z" : "pos_err_z",
00085 "angular.x" : "rot_err_x",
00086 "angular.y" : "rot_err_y",
00087 "angular.z" : "rot_err_z"
00088 }
00089 }
00090 data = {}
00091 for topic in field_map:
00092 for field in field_map[topic]:
00093 data[field_map[topic][field]] = []
00094
00095 bag = rosbag.Bag(sys.argv[1], 'r')
00096 for topic in field_map:
00097 for tp, fp, t in bag.read_messages(topics=[topic]):
00098 for field in field_map[topic]:
00099 exec("data['%s'].append(fp.%s)" % (field_map[topic][field], field))
00100 bag.close()
00101
00102 output_file = sys.argv[1].split(".")[0] + ".mat"
00103 scipy.io.savemat(output_file, data)
00104 print "Saved mat file to:", output_file
00105
00106 if __name__ == "__main__":
00107 main()