dump_mocap.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from geometry_msgs.msg import PoseStamped
00005 from tf.transformations import *
00006 import os
00007 import tf
00008 
00009 initial_pose = None
00010 world_pose = None
00011 def poseMsgToMatrix(pose):
00012     return concatenate_matrices(translation_matrix([pose.position.x,
00013                                                     pose.position.y,
00014                                                     pose.position.z]),
00015                                 quaternion_matrix([pose.orientation.x,
00016                                                    pose.orientation.y,
00017                                                    pose.orientation.z,
00018                                                    pose.orientation.w]))
00019 
00020 
00021 
00022 
00023 def callback(msg):
00024     global f, initial_pose, world_pose
00025     if world_pose == None:
00026         rospy.loginfo("calibrating world coordinates")
00027         A = identity_matrix()
00028         B = poseMsgToMatrix(msg.pose)
00029         C = concatenate_matrices(A, inverse_matrix(B))
00030         world_pose = C
00031     # x, y, z, qx, qy, qz, qw
00032     pose = poseMsgToMatrix(msg.pose)
00033     new_A = concatenate_matrices(world_pose, pose)
00034     diff = new_A
00035     diff_pos = translation_from_matrix(diff)
00036     diff_rot = quaternion_from_matrix(diff)
00037     f.write("%f, %f, %f, %f, %f, %f, %f\n" % (diff_pos[0], diff_pos[1],
00038                                               diff_pos[2],
00039                                               diff_rot[0],
00040                                               diff_rot[1],
00041                                               diff_rot[2],
00042                                               diff_rot[3]))
00043 
00044 if __name__ == "__main__":
00045     rospy.init_node("dump_mocap")
00046     file_name = rospy.get_param("~file_name", "output.csv")
00047     tf_listener = tf.TransformListener()
00048     try:
00049         f = open(file_name, "w")
00050         sub = rospy.Subscriber("/Robot_1/pose", PoseStamped, callback)
00051         rospy.spin()
00052     finally:
00053         f.close()
00054         
00055         
00056     


jsk_footstep_controller
Author(s):
autogenerated on Fri Apr 19 2019 03:45:37