test.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('tf2_kdl')
00004 import rospy
00005 import PyKDL
00006 import tf2_ros
00007 import tf2_kdl
00008 from geometry_msgs.msg import TransformStamped
00009 from copy import deepcopy
00010 
00011 def main():
00012     b = tf2_ros.Buffer()
00013     t = TransformStamped()
00014     t.transform.translation.x = 1
00015     t.transform.rotation.x = 1
00016     t.header.stamp = rospy.Time(2.0)
00017     t.header.frame_id = 'a'
00018     t.child_frame_id = 'b'
00019     b.set_transform(t, 'eitan_rocks')
00020     print b.lookup_transform('a','b', rospy.Time(2.0), rospy.Duration(2.0))
00021 
00022     v = PyKDL.Vector(1,2,3)
00023     print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b')
00024 
00025     f = PyKDL.Frame(PyKDL.Rotation.RPY(1,2,3), PyKDL.Vector(1,2,3))
00026     print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b')
00027 
00028     t = PyKDL.Twist(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
00029     print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b')
00030 
00031     w = PyKDL.Wrench(PyKDL.Vector(1,2,3), PyKDL.Vector(4,5,6))
00032     print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
00033 
00034 def convert():
00035     v = PyKDL.Vector(1,2,3)
00036     vs = tf2_ros.Stamped(v, rospy.Time(2), 'a')
00037     vs2 = tf2_ros.convert(vs, PyKDL.Vector)
00038     vs2[1] = 100
00039     print vs2
00040     print v
00041 
00042 
00043 if __name__ == '__main__':
00044     rospy.init_node('wim')
00045     main()
00046     convert()


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