You're reading the documentation for a development version. For the latest released version, please have a look at Galactic.

Writing a tf2 listener (Python)

Goal: Learn how to use tf2 to get access to frame transformations.

Tutorial level: Intermediate

Time: 10 minutes

Background

In previous tutorials we created a tf2 broadcaster to publish the pose of a turtle to tf2.

In this tutorial we’ll create a tf2 listener to start using tf2.

Prerequisites

This tutorial assumes you have completed the tf2 broadcaster tutorial (Python). In the previous tutorial, we created a learning_tf2_py package, which is where we will continue working from.

Tasks

1 Write the listener node

Let’s first create the source files. Go to the learning_tf2_py package we created in the previous tutorial. Inside the src/learning_tf2_py/learning_tf2_py directory download the example listener code by entering the following command:

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

Open the file using your preferred text editor.

import math

from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

from turtlesim.srv import Spawn


class FrameListener(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_listener')

        # Declare and acquire `target_frame` parameter
        self.declare_parameter('target_frame', 'turtle1')
        self.target_frame = self.get_parameter(
            'target_frame').get_parameter_value().string_value

        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False

        # Create turtle2 velocity publisher
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)

        # Call on_timer function every second
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # Store frame names in variables that will be used to
        # compute transformations
        from_frame_rel = self.target_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                # Look up for the transformation between target_frame and turtle2 frames
                # and send velocity commands for turtle2 to reach target_frame
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

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

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

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

1.1 Examine the code

To understand how the service behind spawning turtle works, please refer to writing a simple service and client (Python) tutorial.

Now, let’s take a look at the code that is relevant to get access to frame transformations. The tf2_ros package provides an implementation of a TransformListener to help make the task of receiving transforms easier.

from tf2_ros.transform_listener import TransformListener

Here, we create a TransformListener object. Once the listener is created, it starts receiving tf2 transformations over the wire, and buffers them for up to 10 seconds.

self.tf_listener = TransformListener(self.tf_buffer, self)

Finally, we query the listener for a specific transformation. We call lookup_transform method with following arguments:

  1. Target frame

  2. Source frame

  3. The time at which we want to transform

Providing rclpy.time.Time() will just get us the latest available transform. All this is wrapped in a try-except block to handle possible exceptions.

now = rclpy.time.Time()
trans = self.tf_buffer.lookup_transform(
    to_frame_rel,
    from_frame_rel,
    now)

2 Build and run

With your text editor, open the launch file called turtle_tf2_demo.launch.py, and add the following lines after your first turtle1 broadcaster node. Additionally, include the imports of DeclareLaunchArgument and LaunchConfiguration in the beginning of the file:

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        ...,
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle2'}
            ]
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_listener',
            name='listener',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ),
    ])

This will declare a target_frame launch argument, start a broadcaster for second turtle that we will spawn and listener that will subscribe to those transformations. Now you’re ready to start your full turtle demo:

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

You should see the turtle sim with two turtles. In the second terminal window type the following command:

ros2 run turtlesim turtle_teleop_key

3 Checking the results

To see if things work, simply drive around the first turtle using the arrow keys (make sure your terminal window is active, not your simulator window), and you’ll see the second turtle following the first one!

Summary

In this tutorial you learned how to use tf2 to get access to frame transformations. You also have finished writing your own turtlesim demo that you first tried in Introduction to tf2 tutorial.