Go to the documentation of this file.00001 import roslib; roslib.load_manifest('iri_common_smach')
00002 import rospy
00003 from geometry_msgs.msg import *
00004
00005 def build_quaternion(qx, qy, qz, qw):
00006 quat = geometry_msgs.msg.Quaternion()
00007 quat.x = qx
00008 quat.y = qy
00009 quat.z = qz
00010 quat.w = qw
00011
00012 return quat
00013
00014 def build_pose(x, y, z, q1, q2, q3, q4):
00015 r = Pose()
00016 r.position = geometry_msgs.msg.Vector3(x,y,z)
00017 r.orientation = build_quaternion(q1, q2, q3, q4)
00018
00019 return r
00020
00021 def build_pose_stamped_msg(frame_id, x, y, z, q1, q2, q3, q4):
00022 r = PoseStamped()
00023 r.header.frame_id = frame_id
00024 r.header.stamp = rospy.Time()
00025 r.pose = build_pose(x, y, z, q1, q2, q3, q4)
00026
00027 return r
00028
00029 def build_transform_msg(x, y, z, qx, qy, qz, qw):
00030 t = geometry_msgs.msg.Transform()
00031 t.translation = geometry_msgs.msg.Vector3(x,y,z)
00032 t.rotation = build_quaternion(qx, qy, qz, qw)
00033
00034 return t