turtle_tf2_broadcaster.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of the Willow Garage nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #!/usr/bin/env python
34 
35 import rospy
36 
37 # Because of transformations
38 import tf
39 
40 import tf2_ros
41 import geometry_msgs.msg
42 import turtlesim.msg
43 
44 
45 def handle_turtle_pose(msg, turtlename):
47  t = geometry_msgs.msg.TransformStamped()
48 
49  t.header.stamp = rospy.Time.now()
50  t.header.frame_id = "world"
51  t.child_frame_id = turtlename
52  t.transform.translation.x = msg.x
53  t.transform.translation.y = msg.y
54  t.transform.translation.z = 0.0
55  q = tf.transformations.quaternion_from_euler(0, 0, msg.theta)
56  t.transform.rotation.x = q[0]
57  t.transform.rotation.y = q[1]
58  t.transform.rotation.z = q[2]
59  t.transform.rotation.w = q[3]
60 
61  br.sendTransform(t)
62 
63 if __name__ == '__main__':
64  rospy.init_node('turtle_tf2_broadcaster')
65  turtlename = rospy.get_param('~turtle')
66  rospy.Subscriber('/%s/pose' % turtlename,
67  turtlesim.msg.Pose,
68  handle_turtle_pose,
69  turtlename)
70  rospy.spin()
def handle_turtle_pose(msg, turtlename)


turtle_tf2
Author(s): Denis Štogl
autogenerated on Wed Jun 5 2019 20:50:43