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


tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:55