00001
00002
00003 import roslib; roslib.load_manifest('tf2_geometry_msgs')
00004 import rospy
00005 import PyKDL
00006 import tf2_ros
00007 import tf2_geometry_msgs
00008 from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped
00009
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 = PointStamped()
00023 v.header.stamp = rospy.Time(2)
00024 v.header.frame_id = 'a'
00025 v.point.x = 1
00026 v.point.y = 2
00027 v.point.z = 3
00028 print b.transform(v, 'b')
00029
00030 v = Vector3Stamped()
00031 v.header.stamp = rospy.Time(2)
00032 v.header.frame_id = 'a'
00033 v.vector.x = 1
00034 v.vector.y = 2
00035 v.vector.z = 3
00036 print b.transform(v, 'b')
00037
00038 v = PoseStamped()
00039 v.header.stamp = rospy.Time(2)
00040 v.header.frame_id = 'a'
00041 v.pose.position.x = 1
00042 v.pose.position.y = 2
00043 v.pose.position.z = 3
00044 v.pose.orientation.x = 1
00045 print b.transform(v, 'b')
00046
00047 if __name__ == '__main__':
00048 rospy.init_node('wim')
00049 main()
