tf2_kdl.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_kdl')
00031 import PyKDL
00032 import rospy
00033 import tf2_ros
00034 from geometry_msgs.msg import PointStamped
00035 
00036 def transform_to_kdl(t):
00037     return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
00038                                                  t.transform.rotation.z, t.transform.rotation.w),
00039                        PyKDL.Vector(t.transform.translation.x, 
00040                                     t.transform.translation.y, 
00041                                     t.transform.translation.z))
00042 
00043 
00044 # Vector
00045 def do_transform_vector(vector, transform):
00046     res = transform_to_kdl(transform) * vector
00047     res.header = transform.header
00048     return res
00049 
00050 tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector)
00051 
00052 def to_msg_vector(vector):
00053     msg = PointStamped()
00054     msg.header = vector.header
00055     msg.point.x = vector[0]
00056     msg.point.y = vector[1]
00057     msg.point.z = vector[2]
00058     return msg
00059 
00060 tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector)
00061 
00062 def from_msg_vector(msg):
00063     vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z)
00064     return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id)
00065 
00066 tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector)
00067 
00068 def convert_vector(vector):
00069     return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id)
00070 
00071 tf2_ros.ConvertRegistration().add_convert((PyKDL.Vector, PyKDL.Vector), convert_vector)
00072 
00073 # Frame
00074 def do_transform_frame(frame, transform):
00075     res = transform_to_kdl(transform) * frame
00076     res.header = transform.header
00077     return res
00078 tf2_ros.TransformRegistration().add(PyKDL.Frame, do_transform_frame)
00079 
00080 # Twist
00081 def do_transform_twist(twist, transform):
00082     res = transform_to_kdl(transform) * twist
00083     res.header = transform.header
00084     return res
00085 tf2_ros.TransformRegistration().add(PyKDL.Twist, do_transform_twist)
00086 
00087 
00088 # Wrench
00089 def do_transform_wrench(wrench, transform):
00090     res = transform_to_kdl(transform) * wrench
00091     res.header = transform.header
00092     return res
00093 tf2_ros.TransformRegistration().add(PyKDL.Wrench, do_transform_wrench)


tf2_kdl
Author(s): Wim Meeussen
autogenerated on Mon Oct 6 2014 00:12:57