tf2_geometry_msgs.py
Go to the documentation of this file.
00001 # Copyright (c) 2008, Willow Garage, Inc.
00002 # All rights reserved.
00003 # 
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions are met:
00006 # 
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00013 #       contributors may be used to endorse or promote products derived from
00014 #       this software without specific prior written permission.
00015 # 
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 # POSSIBILITY OF SUCH DAMAGE.
00027 
00028 # author: Wim Meeussen
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 # PointStamped
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 # Vector3Stamped
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 # PoseStamped
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)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Mon Aug 19 2013 10:27:09