Using tf2

Using `tf2`#

Warning

This topic is under construction and this might not even be its final form. Please feel free to open an issue if you spot any typos or other problems.

Create the package#

Note

The package is called tf2_ros in ROS2.

We start by creating the python_package_that_uses_tf2 package. Please note that it must depend on tf2_ros.

cd ~/ros2_tutorial_workspace/src
ros2 pkg create python_package_that_uses_tf2 \
--build-type ament_python \
--dependencies geometry_msgs tf2_ros

Package structure#

In this section, we will create or modify the following files.

python_package_that_uses_tf2/
|-- package.xml
|-- python_package_that_uses_tf2
|   |-- __init__.py
|   |-- tf2_broadcaster_node.py
|   `-- tf2_listener_node.py
|-- resource
|   `-- python_package_that_uses_tf2
|-- setup.cfg
|-- setup.py
`-- test
    |-- test_copyright.py
    |-- test_flake8.py
    `-- test_pep257.py

Creating the broadcaster#

The broadcaster is made with the following piece of code.

tf2_broadcaster_node.py

from math import sin, cos, pi
from geometry_msgs.msg import TransformStamped

import rclpy
from rclpy.node import Node

import tf2_ros

class TF2BroadcasterNode(Node):
    """A ROS2 Node that broadcasts a TransformStamped in compliance with `tf2_ros2`."""

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

        # Robot name
        self.robot_name = "robot_1"

        # Initialize the transform broadcaster
        ## Note that this object is part of `tf2_ros`, not `rclpy`
        self.transform_broadcaster = tf2_ros.TransformBroadcaster(self)

        self.timer_period: float = 0.01
        self.timer_elapsed_time: float = 0
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def timer_callback(self):
        # We define some parameters of our trajectory.
        # Let's make the translation be a circle in 2D with radius of 0.2 meters and frequency of 0.1 Hz.
        # The rotation will be the robot spinning about the z-axis with the same frequency.
        trajectory_radius = 0.2
        trajectory_frequency = 0.1

        # Create an instance of a TransformStamped, used by the broadcaster
        tfs = TransformStamped()

        ## Initialize header with
        # Timestamp equal to the current clock time
        tfs.header.stamp = self.get_clock().now().to_msg()
        # Frame of reference
        tfs.header.frame_id = 'world'
        # The object whose transform this message is about
        tfs.child_frame_id = self.robot_name

        # Set the translation of the transform as a circle in the x-y plane
        tfs.transform.translation.x = trajectory_radius * cos(2 * pi * trajectory_frequency * self.timer_elapsed_time)
        tfs.transform.translation.y = trajectory_radius * sin(2 * pi * trajectory_frequency * self.timer_elapsed_time)
        tfs.transform.translation.z = 0.0

        # Set the rotation (Quaternion) of the transform as a rotation about the z-axis
        phi: float = 2.0 * pi * trajectory_frequency * self.timer_elapsed_time
        tfs.transform.rotation.w = cos(phi/2.0)
        tfs.transform.rotation.x = 0.0
        tfs.transform.rotation.y = 0.0
        tfs.transform.rotation.z = sin(phi/2.0)

        # Send the transformation
        self.transform_broadcaster.sendTransform(tfs)

        # Update internal time counter
        self.timer_elapsed_time += self.timer_period


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = TF2BroadcasterNode()

        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)

if __name__ == '__main__':
    main()

Most of this should be familiar by now.

Focusing on the main novelty, we create the broadcaster with tf2_ros.TransformBroadcaster. We need to input a rclpy.node.Node for the constructor, so we input it as self.

As minor things, we have a tag for this frame as robot_name. This could, for example, be a ros2. We leave it hard-coded for simplicity. We also keep time in the object’s scope with timer_elapsed_time so that it persists across callback calls.

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

        # Robot name
        self.robot_name = "robot_1"

        # Initialize the transform broadcaster
        ## Note that this object is part of `tf2_ros`, not `rclpy`
        self.transform_broadcaster = tf2_ros.TransformBroadcaster(self)

        self.timer_period: float = 0.01
        self.timer_elapsed_time: float = 0
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

We add two parameters to be able to adjust the trajectory properties if desired. Again, these could be configurable parameters but we leave them hard-coded for simplicity.

        # We define some parameters of our trajectory.
        # Let's make the translation be a circle in 2D with radius of 0.2 meters and frequency of 0.1 Hz.
        # The rotation will be the robot spinning about the z-axis with the same frequency.
        trajectory_radius = 0.2
        trajectory_frequency = 0.1

We create the TransformStamped as follows. Our rotation is simple so we build Quaternion for the rotation directly. We set the reference frame, header.frame_id, as world and child_frame_id as this robot’s frame.

        tfs = TransformStamped()

        ## Initialize header with
        # Timestamp equal to the current clock time
        tfs.header.stamp = self.get_clock().now().to_msg()
        # Frame of reference
        tfs.header.frame_id = 'world'
        # The object whose transform this message is about
        tfs.child_frame_id = self.robot_name

        # Set the translation of the transform as a circle in the x-y plane
        tfs.transform.translation.x = trajectory_radius * cos(2 * pi * trajectory_frequency * self.timer_elapsed_time)
        tfs.transform.translation.y = trajectory_radius * sin(2 * pi * trajectory_frequency * self.timer_elapsed_time)
        tfs.transform.translation.z = 0.0

        # Set the rotation (Quaternion) of the transform as a rotation about the z-axis
        phi: float = 2.0 * pi * trajectory_frequency * self.timer_elapsed_time
        tfs.transform.rotation.w = cos(phi/2.0)
        tfs.transform.rotation.x = 0.0
        tfs.transform.rotation.y = 0.0
        tfs.transform.rotation.z = sin(phi/2.0)

Finally, we send the transform with transform_broadcaster.sendTransform and update the local time counter for the trajectory.

        # Send the transformation
        self.transform_broadcaster.sendTransform(tfs)

        # Update internal time counter
        self.timer_elapsed_time += self.timer_period

Creating the listener#

The listener is made with the following piece of code.

tf2_listener_node.py

import rclpy
from rclpy.node import Node

import tf2_ros

class TF2ListenerNode(Node):
    """A ROS2 Node that listens to the `tf` topic in compliance with `tf2_ros2`."""

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

        # Setting up the TransformListener
        self.transform_listener_buffer = tf2_ros.Buffer()
        self.transform_listener = tf2_ros.TransformListener(self.transform_listener_buffer, self)

        # Information about the transform we want to listen to
        self.parent_name = "world"
        self.child_name = "robot_1"

        self.timer_period: float = 0.1
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

    def timer_callback(self):

        try:
            tfs =   self.transform_listener_buffer.lookup_transform(
                    self.parent_name,
                    self.child_name,
                    rclpy.time.Time())

            self.get_logger().info(f"Transform: {tfs}")

        except tf2_ros.TransformException as e:

            self.get_logger().error(
                f'Could not get transform from `{self.parent_name}` to `{self.child_name}`: {e}')


def main(args=None):
    """
    The main function.
    :param args: Not used directly by the user, but used by ROS2 to configure certain aspects of the Node.
    """
    try:
        rclpy.init(args=args)

        node = TF2ListenerNode()

        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(e)

if __name__ == '__main__':
    main()

We use an instance of tf2_ros.TransformListener to manage the interaction with tf2 for us. The only step that might be unfamiliar is that you need to create an instance to a buffer which is used in the initializer of TransformListener.

We also add the frame information that we want to listen to. This will be used in another step. One extension of this would be to make this configurable as a parameter. We do not do that here to not increase complexity.

        # Setting up the TransformListener
        self.transform_listener_buffer = tf2_ros.Buffer()
        self.transform_listener = tf2_ros.TransformListener(self.transform_listener_buffer, self)

        # Information about the transform we want to listen to
        self.parent_name = "world"
        self.child_name = "robot_1"

We then create an instance of Timer and add the callback. In the callback, we use the buffer we created in the previous step. We will call lookup_transform and it will need the parent frame tag, the child name tag, and the time of lookup. We add exception handling in case the transform is not available or not available in the time requested.

The object created with rclpy.time.Time() is equivalent to a time of zero, and lookup_transform returns the latest transformation available.

    def timer_callback(self):

        try:
            tfs =   self.transform_listener_buffer.lookup_transform(
                    self.parent_name,
                    self.child_name,
                    rclpy.time.Time())

            self.get_logger().info(f"Transform: {tfs}")

        except tf2_ros.TransformException as e:

            self.get_logger().error(
                f'Could not get transform from `{self.parent_name}` to `{self.child_name}`: {e}')

The `setup.py`#

We add the usual entry points.

setup.py

from setuptools import find_packages, setup

package_name = 'python_package_that_uses_tf2'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='murilo.marinho@manchester.ac.uk',
    description='TODO: Package description',
    license='TODO: License declaration',
    extras_require={
        'test': [
            'pytest',
        ],
    },
    entry_points={
        'console_scripts': [
            "tf2_broadcaster_node = python_package_that_uses_tf2.tf2_broadcaster_node:main",
            "tf2_listener_node = python_package_that_uses_tf2.tf2_listener_node:main"
        ],
    },
)

Build and source#

Before we proceed, let us build and source once.

cd ~/ros2_tutorial_workspace
colcon build
source install/setup.bash

Note

For additional explanation and troubleshooting tips, see Always source after you build.

Warning

colcon will not work properly if your terminal has an active venv.

Run Example#

We can show the integrated example as follows.

Terminal 1: tf2 broadcaster

We start by running the broadcaster.

ros2 run python_package_that_uses_tf2 tf2_broadcaster_node

Terminal 2: tf2 listener

Then, we run the listener.

ros2 run python_package_that_uses_tf2 tf2_listener_node

Our broadcaster won’t output anything to the screen.

The output of the listener will be as long as you allow it to be. The first line indicates a normal behavior, in which subscribers to not have instant access to topics. That is properly handled by our listener. When available, you can see the output.

[ERROR] [1762112353.248708886] [tf2_listener_node]: Could not transform world to robot_1: "world" passed to lookupTransform argument target_frame does not exist.
[INFO] [1762112353.340386011] [tf2_listener_node]: Transform: geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1762112353, nanosec=335937928), frame_id='world'), child_frame_id='robot_1', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.11448642511890406, y=-0.16399042186510032, z=0.0), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=-0.46236775104102995, w=0.8866882557005366)))
[INFO] [1762112353.440609636] [tf2_listener_node]: Transform: geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1762112353, nanosec=436304803), frame_id='world'), child_frame_id='robot_1', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.12455755609760884, y=-0.1564781621153285, z=0.0), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=-0.43428804928984416, w=0.9007740506053791)))

For now, you have to believe I’m right and that this means the broadcaster is sending a circular trajectory. We will see more about this in visualization and simulation.

What is happening?#

The broadcaster and listener are communicating through a ROS2 topic. We can check that with the following command. Please ensure that both nodes are running before checking this.

ros2 topic list

This should output the following.

/parameter_events
/rosout
/tf

We can obtain more information about the /tf topic with the following command.

ros2 topic info /tf

The output of the command should be as follows.

Type: tf2_msgs/msg/TFMessage
Publisher count: 1
Subscription count: 1

You might be asking yourself the following.

What nonsense is this? Wasn’t it supposed to be a geometry_msgs/msg/TransformStamped?? I also didn’t define no topic /tf?????

You will notice that although tf2 uses topics, publishers, and subscribers, it has another layer of complexity. For instance, most of the time we can expect many frames (think of dozens or hundreds) to be published in the same topic, making a regular subscriber not as useful and too busy.

The broadcaster and listener, although using a geometry_msgs/msg/TransformStamped in our nodes, will convert back-and-forth between tf2_msgs/msg/TFMessage and geometry_msgs/msg/TransformStamped when interacting with the topic /tf. Also, because of that, you can think of the topic /tf as a protected topic. No such concept of protected topics seems to exist in ROS2, so that is not enforced, you can publish and subscribe normally to that topic. Basically, in this context, I mean to say that if you attempt to use /tf without using tf2_ros facilities, you are likely to be disappointed.

Either way, there is no reason to fear. You will notice that tf2_msgs/msg/TFMessage is simply an array of geometry_msgs/msg/TransformStamped.

You can see that with the following command.

ros2 interface show tf2_msgs/msg/TFMessage

This should result in the following, where the important line is highlighted.

geometry_msgs/TransformStamped[] transforms
        #
        #
        std_msgs/Header header
                builtin_interfaces/Time stamp
                        int32 sec
                        uint32 nanosec
                string frame_id
        string child_frame_id
        Transform transform
                Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                Quaternion rotation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1

There will be a degree of pointlessness when attempting to parse through the /tf output in the terminal. We will also not attempt to do that here. To help you with that, we will have visualisation and simulation tools shown in following sections.