Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import StringIO
00036 from moveit_commander import MoveItCommanderException
00037 from geometry_msgs.msg import Pose, PoseStamped, Transform
00038 import rospy
00039 import tf
00040
00041 def msg_to_string(msg):
00042 buf = StringIO.StringIO()
00043 msg.serialize(buf)
00044 return buf.getvalue()
00045
00046 def msg_from_string(msg, data):
00047 msg.deserialize(data)
00048
00049 def pose_to_list(pose_msg):
00050 pose = []
00051 pose.append(pose_msg.position.x)
00052 pose.append(pose_msg.position.y)
00053 pose.append(pose_msg.position.z)
00054 pose.append(pose_msg.orientation.x)
00055 pose.append(pose_msg.orientation.y)
00056 pose.append(pose_msg.orientation.z)
00057 pose.append(pose_msg.orientation.w)
00058 return pose
00059
00060 def list_to_pose(pose_list):
00061 pose_msg = Pose()
00062 if len(pose_list) == 7:
00063 pose_msg.position.x = pose_list[0]
00064 pose_msg.position.y = pose_list[1]
00065 pose_msg.position.z = pose_list[2]
00066 pose_msg.orientation.x = pose_list[3]
00067 pose_msg.orientation.y = pose_list[4]
00068 pose_msg.orientation.z = pose_list[5]
00069 pose_msg.orientation.w = pose_list[6]
00070 elif len(pose_list) == 6:
00071 pose_msg.position.x = pose_list[0]
00072 pose_msg.position.y = pose_list[1]
00073 pose_msg.position.z = pose_list[2]
00074 q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5])
00075 pose_msg.orientation.x = q[0]
00076 pose_msg.orientation.y = q[1]
00077 pose_msg.orientation.z = q[2]
00078 pose_msg.orientation.w = q[3]
00079 else:
00080 raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)")
00081 return pose_msg
00082
00083 def list_to_pose_stamped(pose_list, target_frame):
00084 pose_msg = PoseStamped()
00085 pose_msg.pose = list_to_pose(pose_list)
00086 pose_msg.header.frame_id = target_frame
00087 pose_msg.header.stamp = rospy.Time.now()
00088 return pose_msg
00089
00090 def transform_to_list(trf_msg):
00091 trf = []
00092 trf.append(trf_msg.translation.x)
00093 trf.append(trf_msg.translation.y)
00094 trf.append(trf_msg.translation.z)
00095 trf.append(trf_msg.rotation.x)
00096 trf.append(trf_msg.rotation.y)
00097 trf.append(trf_msg.rotation.z)
00098 trf.append(trf_msg.rotation.w)
00099 return trf
00100
00101 def list_to_transform(trf_list):
00102 trf_msg = Transform()
00103 trf_msg.translation.x = trf_list[0]
00104 trf_msg.translation.y = trf_list[1]
00105 trf_msg.translation.z = trf_list[2]
00106 trf_msg.rotation.x = trf_list[3]
00107 trf_msg.rotation.y = trf_list[4]
00108 trf_msg.rotation.z = trf_list[5]
00109 trf_msg.rotation.w = trf_list[6]
00110 return trf_msg