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 import roslib; roslib.load_manifest('tf2_geometry_msgs')
00031 from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped
00032 import PyKDL
00033 import rospy
00034 import tf2_ros
00035
00036 def to_msg_msg(msg):
00037 return msg
00038
00039 tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg)
00040 tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg)
00041 tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg)
00042
00043 def from_msg_msg(msg):
00044 return msg
00045
00046 tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg)
00047 tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg)
00048 tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg)
00049
00050 def transform_to_kdl(t):
00051 return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
00052 t.transform.rotation.z, t.transform.rotation.w),
00053 PyKDL.Vector(t.transform.translation.x,
00054 t.transform.translation.y,
00055 t.transform.translation.z))
00056
00057
00058
00059 def do_transform_point(point, transform):
00060 p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z)
00061 res = PointStamped()
00062 res.point.x = p[0]
00063 res.point.y = p[1]
00064 res.point.z = p[2]
00065 res.header = transform.header
00066 return res
00067 tf2_ros.TransformRegistration().add(PointStamped, do_transform_point)
00068
00069
00070
00071 def do_transform_vector3(vector3, transform):
00072 p = transform_to_kdl(transform) * PyKDL.Vector(vector3.vector.x, vector3.vector.y, vector3.vector.z)
00073 res = Vector3Stamped()
00074 res.vector.x = p[0]
00075 res.vector.y = p[1]
00076 res.vector.z = p[2]
00077 res.header = transform.header
00078 return res
00079 tf2_ros.TransformRegistration().add(Vector3Stamped, do_transform_vector3)
00080
00081
00082 def do_transform_pose(pose, transform):
00083 f = transform_to_kdl(transform) * PyKDL.Frame(PyKDL.Rotation.Quaternion(pose.pose.orientation.x, pose.pose.orientation.y,
00084 pose.pose.orientation.z, pose.pose.orientation.w),
00085 PyKDL.Vector(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z))
00086 res = PoseStamped()
00087 res.pose.position.x = f.p[0]
00088 res.pose.position.y = f.p[1]
00089 res.pose.position.z = f.p[2]
00090 (res.pose.orientation.x, res.pose.orientation.y, res.pose.orientation.z, res.pose.orientation.w) = f.M.GetQuaternion()
00091 res.header = transform.header
00092 return res
00093 tf2_ros.TransformRegistration().add(PoseStamped, do_transform_pose)