tf/tf2 Broadcaster and Listener in Python

July 13, 2021 by Abdur Rosyid

tf Broadcaster and Listener

tf broadcaster node written in Python can be found here:

#!/usr/bin/env python

import roslib

roslib.load_manifest('learning_tf')

import rospy

import tf

import turtlesim.msg

def handle_turtle_pose(msg, turtlename):

br = tf.TransformBroadcaster()

br.sendTransform((msg.x, msg.y, 0),

tf.transformations.quaternion_from_euler(0, 0, msg.theta),

rospy.Time.now(),

turtlename,

"world")

if __name__ == '__main__':

rospy.init_node('turtle_tf_broadcaster')

turtlename = rospy.get_param('~turtle')

rospy.Subscriber('/%s/pose' % turtlename,

turtlesim.msg.Pose,

handle_turtle_pose,

turtlename)

rospy.spin()

In the code above, the node is subscribing to the pose of robot given by “~turtle” parameter. Upon receiving the pose topic, it will run “handle_turtle_pose” callback function which basically broadcast the transformation between the robot frame and the world frame.

tf listener node in Python can be found here:

#!/usr/bin/env python  
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

listener = tf.TransformListener()

rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

rate.sleep()

#!/usr/bin/env python

import roslib

roslib.load_manifest('learning_tf')

import rospy

import math

import tf

import geometry_msgs.msg

import turtlesim.srv

if __name__ == '__main__':

rospy.init_node('turtle_tf_listener')

listener = tf.TransformListener()

rospy.wait_for_service('spawn')

spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)

spawner(4, 2, 0, 'turtle2')

turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

rate = rospy.Rate(10.0)

while not rospy.is_shutdown():

try:

(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

continue

angular = 4 * math.atan2(trans[1], trans[0])

linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)

cmd = geometry_msgs.msg.Twist()

cmd.linear.x = linear

cmd.angular.z = angular

turtle_vel.publish(cmd)

rate.sleep()

The following are the most important parts:

1	listener = tf.TransformListener()

This is declaring a variable representing “tf.TransformListener()”.

while not rospy.is_shutdown():

try:

(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

continue

This is to perform “tf.TransformListener.lookupTransform”, which basically listen the transformation, between “turtle1” frame and “turtle2” frame, which are returned as (trans, rot). The (trans, rot) values are then fed into some mathematics to provide the required linear and angular velocity commands to move the robot.

tf2 Broadcaster and Listener

The tf2 ways to achieve the same things as described above (using tf) are below.

tf2 broadcaster node in Python can be found here:

#!/usr/bin/env python  
import rospy

# Because of transformations
import tf_conversions

import tf2_ros
import geometry_msgs.msg
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

t.header.stamp = rospy.Time.now()
    t.header.frame_id = "world"
    t.child_frame_id = turtlename
    t.transform.translation.x = msg.x
    t.transform.translation.y = msg.y
    t.transform.translation.z = 0.0
    q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

br.sendTransform(t)

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

#!/usr/bin/env python

import rospy

# Because of transformations

import tf_conversions

import tf2_ros

import geometry_msgs.msg

import turtlesim.msg

def handle_turtle_pose(msg, turtlename):

br = tf2_ros.TransformBroadcaster()

t = geometry_msgs.msg.TransformStamped()

t.header.stamp = rospy.Time.now()

t.header.frame_id = "world"

t.child_frame_id = turtlename

t.transform.translation.x = msg.x

t.transform.translation.y = msg.y

t.transform.translation.z = 0.0

q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)

t.transform.rotation.x = q[0]

t.transform.rotation.y = q[1]

t.transform.rotation.z = q[2]

t.transform.rotation.w = q[3]

br.sendTransform(t)

if __name__ == '__main__':

rospy.init_node('tf2_turtle_broadcaster')

turtlename = rospy.get_param('~turtle')

rospy.Subscriber('/%s/pose' % turtlename,

turtlesim.msg.Pose,

handle_turtle_pose,

turtlename)

rospy.spin()

tf2 listener node in Python can be found here:

#!/usr/bin/env python  
import rospy

import math
import tf2_ros
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_listener')

tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer)

rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    turtle_name = rospy.get_param('turtle', 'turtle2')
    spawner(4, 2, 0, turtle_name)

turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name, geometry_msgs.msg.Twist, queue_size=1)

rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rate.sleep()
            continue

msg = geometry_msgs.msg.Twist()

msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)

turtle_vel.publish(msg)

rate.sleep()

#!/usr/bin/env python

import rospy

import math

import tf2_ros

import geometry_msgs.msg

import turtlesim.srv

if __name__ == '__main__':

rospy.init_node('tf2_turtle_listener')

tfBuffer = tf2_ros.Buffer()

listener = tf2_ros.TransformListener(tfBuffer)

rospy.wait_for_service('spawn')

spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)

turtle_name = rospy.get_param('turtle', 'turtle2')

spawner(4, 2, 0, turtle_name)

turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name, geometry_msgs.msg.Twist, queue_size=1)

rate = rospy.Rate(10.0)

while not rospy.is_shutdown():

try:

trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())

except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):

rate.sleep()

continue

msg = geometry_msgs.msg.Twist()

msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)

turtle_vel.publish(msg)

rate.sleep()

Leave a Reply Cancel reply