Writing a broadcaster (Python)

Goal: Learn how to broadcast the state of a robot to tf2.

Tutorial level: Intermediate

Time: 15 minutes

Background

In the next two tutorials we will write the code to reproduce the demo from the Introduction to tf2 tutorial. After that, following tutorials focus on extending the demo with more advanced tf2 features, including the usage of timeouts in transformation lookups and time travel.

Prerequisites

This tutorial assumes you have a working knowledge of ROS 2 and you have completed the Introduction to tf2 tutorial. In previous tutorials, you learned how to create a workspace and create a package. You also have created the learning_tf2_py package, which is where we will continue working from.

Tasks

1 Write the broadcaster 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 broadcaster code by entering the following command:

LinuxmacOSWindows

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

Open the file using your preferred text editor.

import math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class FramePublisher(Node):

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

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = 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]

        # Send the transformation
        self.tf_broadcaster.sendTransform(t)


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

    rclpy.shutdown()

1.1 Examine the code

Now, let’s take a look at the code that is relevant to publishing the turtle pose to tf2. Firstly, we define and acquire a single parameter turtlename, which specifies a turtle name, e.g. turtle1 or turtle2.

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

Afterward, the node subscribes to topic turtleX/pose and runs function handle_turtle_pose on every incoming message.

self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

Now, we create a TransformStamped object and give it the appropriate metadata.

1. We need to give the transform being published a timestamp, and we’ll just stamp it with the current time by calling self.get_clock().now(). This will return the current time used by the Node.

2. Then we need to set the name of the parent frame of the link we’re creating, in this case world.

3. Finally, we need to set the name of the child node of the link we’re creating, in this case this is the name of the turtle itself.

The handler function for the turtle pose message broadcasts this turtle’s translation and rotation, and publishes it as a transform from frame world to frame turtleX.

t = TransformStamped()

# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

Here we copy the information from the 3D turtle pose into the 3D transform.

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0

# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = 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]

Finally we take the transform that we constructed and pass it to the sendTransform method of the TransformBroadcaster that will take care of broadcasting.

# Send the transformation
self.tf_broadcaster.sendTransform(t)

Nota

You can also publish static transforms with the same pattern by instantiating a tf2_ros.StaticTransformBroadcaster instead of a tf2_ros.TransformBroadcaster. The static transforms will be published on the /tf_static topic and will be sent only when required, not periodically. For more details see here.

1.2 Add an entry point

To allow the ros2 run command to run your node, you must add the entry point to setup.py (located in the src/learning_tf2_py directory).

Finally, add the following line between the 'console_scripts': brackets:

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2 Write the launch file

Now create a launch file for this demo. With your text editor, create a new file called turtle_tf2_demo.launch.py in the launch folder, and add the following lines:

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

2.1 Examine the code

First we import required modules from the launch and launch_ros packages. It should be noted that launch is a generic launching framework (not ROS 2 specific) and launch_ros has ROS 2 specific things, like nodes that we import here.

from launch import LaunchDescription
from launch_ros.actions import Node

Now we run our nodes that start the turtlesim simulation and broadcast turtle1 state to the tf2 using our turtle_tf2_broadcaster node.

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_py',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

2.2 Add dependencies

Navigate one level back to the src/learning_tf2_py directory, where the setup.py, setup.cfg, and package.xml files are located.

Open package.xml with your text editor. Add the following dependencies corresponding to your launch file’s import statements:

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

This declares the additional required launch and launch_ros dependencies when its code is executed.

Make sure to save the file.

2.3 Update setup.py

Reopen setup.py and add the line so that the launch files from the launch/ folder would be installed. The data_files field should now look like this:

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

Also add the appropriate imports at the top of the file:

import os
from glob import glob

You can learn more about creating launch files in this tutorial.

3 Build

Run rosdep in the root of your workspace to check for missing dependencies.

LinuxmacOSWindows

rosdep install -i --from-path src --rosdistro humble -y

Still in the root of your workspace, build your package:

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

Open a new terminal, navigate to the root of your workspace, and source the setup files:

LinuxmacOSWindows

. install/setup.bash

4 Run

Now run the launch file that will start the turtlesim simulation node and turtle_tf2_broadcaster node:

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

In the second terminal window type the following command:

ros2 run turtlesim turtle_teleop_key

You will now see that the turtlesim simulation have started with one turtle that you can control.

Now, use the tf2_echo tool to check if the turtle pose is actually getting broadcast to tf2:

ros2 run tf2_ros tf2_echo world turtle1

This should show you the pose of the first turtle. Drive around the turtle using the arrow keys (make sure your turtle_teleop_key terminal window is active, not your simulator window). In your console output you will see something similar to this:

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

If you run tf2_echo for the transform between the world and turtle2, you should not see a transform, because the second turtle is not there yet. However, as soon as we add the second turtle in the next tutorial, the pose of turtle2 will be broadcast to tf2.

Summary

In this tutorial you learned how to broadcast the pose of the robot (position and orientation of the turtle) to tf2 and how to use the tf2_echo tool. To actually use the transforms broadcasted to tf2, you should move on to the next tutorial about creating a tf2 listener.