Source Code for Module tf.broadcaster

 1  # Software License Agreement (BSD License) 
 2  # 
 3  # Copyright (c) 2008, Willow Garage, Inc. 
 4  # All rights reserved. 
 5  # 
 6  # Redistribution and use in source and binary forms, with or without 
 7  # modification, are permitted provided that the following conditions 
 8  # are met: 
 9  # 
10  #  * Redistributions of source code must retain the above copyright 
11  #    notice, this list of conditions and the following disclaimer. 
12  #  * Redistributions in binary form must reproduce the above 
13  #    copyright notice, this list of conditions and the following 
14  #    disclaimer in the documentation and/or other materials provided 
15  #    with the distribution. 
16  #  * Neither the name of the Willow Garage nor the names of its 
17  #    contributors may be used to endorse or promote products derived 
18  #    from this software without specific prior written permission. 
19  # 
20  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
21  # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
22  # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
23  # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
24  # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
25  # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
26  # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
27  # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
28  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
29  # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
30  # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
31  # POSSIBILITY OF SUCH DAMAGE. 
32   
33  import rospy 
34  import tf 
35  import tf.msg 
36  import geometry_msgs.msg 
37  import math 
38   
39 -class TransformBroadcaster: 
40      """ 
41      :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic. 
42      """ 
43   
44 -    def __init__(self): 
45          self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage) 
46   
47 -    def sendTransform(self, translation, rotation, time, child, parent): 
48          """ 
49          :param translation: the translation of the transformtion as a tuple (x, y, z) 
50          :param rotation: the rotation of the transformation as a tuple (x, y, z, w) 
51          :param time: the time of the transformation, as a rospy.Time() 
52          :param child: child frame in tf, string 
53          :param parent: parent frame in tf, string 
54   
55          Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``. 
56          """ 
57   
58          t = geometry_msgs.msg.TransformStamped() 
59          t.header.frame_id = parent 
60          t.header.stamp = time 
61          t.child_frame_id = child 
62          t.transform.translation.x = translation[0] 
63          t.transform.translation.y = translation[1] 
64          t.transform.translation.z = translation[2] 
65   
66          t.transform.rotation.x = rotation[0] 
67          t.transform.rotation.y = rotation[1] 
68          t.transform.rotation.z = rotation[2] 
69          t.transform.rotation.w = rotation[3] 
70   
71          tfm = tf.msg.tfMessage([t]) 
72          self.pub_tf.publish(tfm) 
73   
74  if __name__ == '__main__': 
75      rospy.init_node('tf_turtle') 
76      tfb = TurtleTFBroadcaster(rospy.get_param('~turtle')) 
77      rospy.spin() 
78