9

Transform Library

9.1 A Gentle Introduction

tf2 is the transform library , which lets the user keep track of multiple coordinate frames over time. tf2 maintains the relationship between coordinate frames in a tree structure buffered in time and lets the user transform points, vectors, etc., between any two (2) coordinate frames at any desired point in time.

A robotic system typically various types of 3D coordinate frames which change over time.

The key use-case for tf2 is to keep track of all these frames over time, and allows us to ask questions like:

• Where was the head frame relative to the world frame 5 seconds ago?

• What is the pose of the object in my gripper relative to my base?

• What is the current pose of the base frame in the map frame?

tf2 can operate in a distributed system. This means all the information about the coordinate frames of a robot is available to all ROS components on any computer in the system. In addition, tf2 can have every component in your distributed system build its own transform information database or have a central node that gathers and stores all transform information.

A Simple Test

To get started with the tf2 library, we are going to run a demo and see some of the power of tf2 in a multi-robot example using turtlesim, which we should already be familiar with.

Installing the Demo We will start by installing the demo package and its dependencies, like we did previously. This might have been already installed depending on the way ROS was installed, but it doesn’t hurt to run the command again:

sudo apt-get install \ 
     ros-humble-rviz2 \ 
     ros-humble-turtle-tf2-py \ 
     ros-humble-tf2-ros \ 
     ros-humble-tf2-tools \ 
     ros-humble-turtlesim

Now that we’ve installed the turtle_tf2_py tutorial package let’s run the demo. First, open a new terminal and source your ROS installation so that ros2 commands will work. ^{1 1} While the docker container setup has the source predefined, it is nevertheless a good practice to do this if you are working with multiple versions. Then run the following command:

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

You will see the turtlesim start with two (2) turtles. In the second terminal window type the following command:

ros2 run turtlesim turtle_teleop_key

Once the turtlesim is started you can drive the central turtle around in the turtlesim using the keyboard arrow keys, select the second terminal window so that our keystrokes will be captured to drive the turtle.

You can see that one turtle continuously moves to follow the turtle you are driving around.

Looking at the Behaviour This demo is using the tf2 library to create three (3) coordinate frames:

1.

a world frame,

2.

a turtle1 frame, and

3.

a turtle2 frame.

In this tutorial, we use a tf2 broadcaster to publish the turtle coordinate frames and a tf2 listener to calculate the difference in the turtle frames and move one turtle to follow the other.

Useful Tools

It is time to see how tf2 is being used to create this simple exercise. We can use tf2_tools to look at what tf2 is doing behind the scenes.

Viewing the Frames If we want to see visually what is going on, we can use view_frames which creates a diagram of the frames being broadcast by tf2 over ROS .

ros2 run tf2_tools view_frames

Listening to tf data during 5 seconds... 
Generating graph in frames.pdf file...

Here a tf2 listener is listening to the frames which are being broadcast over ROS and drawing a tree of how the frames are connected.

Here we can see three (3) frames that are broadcast by tf2 :

The world frame is the parent of the turtle1 and turtle2 frames. view_frames also reports some diagnostic information about when the oldest and most recent frame transforms were received and how fast the tf2 frame is published to tf2 for debugging purposes.

Using Echo tf2_echo reports the transform between any two (2) frames broadcast over ROS . The syntax of this command is as follows:

ros2 run tf2_ros tf2_echo [source_frame] [target_frame]

Let’s look at the transform of the turtle2 frame with respect to turtle1 frame which is equivalent to:

ros2 run tf2_ros tf2_echo turtle2 turtle1

At time 1683385337.850619099 
- Translation: [2.157, 0.901, 0.000] 
- Rotation: in Quaternion [0.000, 0.000, 0.172, 0.985] 
- Rotation: in RPY (radian) [0.000, -0.000, 0.345] 
- Rotation: in RPY (degree) [0.000, -0.000, 19.760] 
- Matrix: 
  0.941 -0.338  0.000  2.157 
  0.338  0.941  0.000  0.901 
  0.000  0.000  1.000  0.000 
  0.000  0.000  0.000  1.000 
At time 1683385338.841997774 
- Translation: [1.256, 0.216, 0.000] 
- Rotation: in Quaternion [0.000, 0.000, -0.016, 1.000] 
- Rotation: in RPY (radian) [0.000, 0.000, -0.032] 
- Rotation: in RPY (degree) [0.000, 0.000, -1.839] 
- Matrix: 
  0.999  0.032  0.000  1.256 
 -0.032  0.999 -0.000  0.216 
 -0.000  0.000  1.000  0.000 
  0.000  0.000  0.000  1.000

We will see the transform displayed as the tf2_echo listener receives the frames broadcast over ROS . As we drive our turtle around we will see the transform change as the two (2) turtles move relative to each other.

rviz2 and tf2 rviz2 is a three-dimensional visualization platform in ROS . On the one hand, it can realize the graphical display of external information, and on the other hand, it can also release control information to the object through rviz, so as to realize the monitoring and control of the robot. It is also useful for examining tf2 frames. Let’s look at our turtle frames using rviz2 by starting it with a configuration file using the -d option:

ros2 run rviz2 rviz2 -d \ 
     $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

In the side bar you will see the frames broadcasted by tf2. As you drive the turtle around you will see the frames move in rviz.

9.2 Writing a Static Broadcaster

Publishing static transforms is useful to define the relationship between a robot base and its sensors or non-moving parts.

In this section, we will be covering the basics of static transforms, which consists of two (2) parts:

• In the first part we will write code to publish static transforms to tf2.

• In the second part we will explain how to use the command-line static_transform_publisher executable tool in tf2_ros.

In the next two (2) sections we will write the code to reproduce the exercise we did at the beginning of the chapter.

Creating the Package As with almost all ROS applications, we first need to create a package will be used for this section and the following ones. The package called learning_tf2_py will depend on:

Open a new terminal and source your ROS installation so that ros2 commands will work. Navigate to workspace’s src folder and create a new package:

ros2 pkg create --build-type ament_python \ 
     --license Apache-2.0 \ 
     -- learning_tf2_py

Once the command has been executed, our terminal will return a message verifying the creation of our package learning\_tf2\_py and all its necessary files and folders.

Writing a Static Broadcaster Node We will start, of course, by first creating the source files. Within the [ /]src/learning_tf2_py/learning_tf2_py directory download the example static broadcaster code by entering the following command:

wget https://raw.githubusercontent.com/ros/geometry_tutorials/humble/turtle_tf2_py/turtle_tf2_py/static_turtle_tf2_broadcaster.py

Now let’s open the file called static\_turtle\_tf2\_broadcaster.py and have a look at it.

Looking at the Code Now let’s look at the code that is relevant to publishing the static turtle pose to tf2. The first lines import required packages. First we import the TransformStamped from the geometry_msgs, which provides us a template for the message that we will publish to the transformation tree.

from geometry_msgs.msg import TransformStamped

Afterwards, rclpy is imported so its Node class can be used.

import rclpy 
from rclpy.node import Node

The tf2_ros package provides a StaticTransformBroadcaster to make the publishing of static transforms easy. To use the StaticTransformBroadcaster, we need to import it from the tf2_ros module.

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

The StaticFramePublisher class constructor initializes the node with the name static_turtle_tf2_broadcaster. Then, StaticTransformBroadcaster is created, which will send one static transformation upon the startup.

self.tf_static_broadcaster = StaticTransformBroadcaster(self) 
self.make_transforms(transformation)

Here we create a TransformStamped object, which will be the message we will send over once populated. Before passing the actual transform values we need to give it the appropriate metadata.

• We need to give the transform being published a timestamp and well just stamp it with the current time, self.get_clock().now()

• Then we need to set the name of the parent frame of the link were creating, in this case world.

• Finally, we need to set the name of the child frame of the link were creating

t = TransformStamped() 
 
t.header.stamp = self.get_clock().now().to_msg() 
t.header.frame_id = 'world' 
t.child_frame_id = transformation[1]

Here we populate the 6D pose (translation and rotation) of the turtle.

t.transform.translation.x = float(transformation[2]) 
t.transform.translation.y = float(transformation[3]) 
t.transform.translation.z = float(transformation[4]) 
quat = quaternion_from_euler( 
   float(transformation[5]), float(transformation[6]), float(transformation[7])) 
t.transform.rotation.x = quat[0] 
t.transform.rotation.y = quat[1] 
t.transform.rotation.z = quat[2] 
t.transform.rotation.w = quat[3]

Finally, we broadcast static transform using the sendTransform() function.

self.tf_static_broadcaster.sendTransform(t)

Updating the Package Information Navigate one level back to the src/learning\_tf2\_py directory, where the setup.py, setup.cfg, and package.xml files have been created for you.

Open package.xml and make sure to fill in the <description>, <maintainer> and <license> tags:

<description>Learning tf2 with rclpy</description> 
<maintainer email="you@email.com">Your Name</maintainer> 
<license>Apache License 2.0</license>

After the lines above, add the following dependencies corresponding to your nodes import statements:

<exec_depend>geometry_msgs</exec_depend> 
<exec_depend>python3-numpy</exec_depend> 
<exec_depend>rclpy</exec_depend> 
<exec_depend>tf2_ros_py</exec_depend> 
<exec_depend>turtlesim</exec_depend>

This declares the required geometry_msgs, python3-numpy, rclpy, tf2_ros_py, and turtlesim dependencies when its code is executed.

Adding an Entry Point To allow the ros2 run command to run your node, you must add the entry point to setup.py. Add the following line between the 'console_scripts': brackets:

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

Build Its good practice to run rosdep in the root of your workspace to check for missing dependencies before building:

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

Run Now run the static_turtle_tf2_broadcaster node:

ros2 run \ 
     learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

This sets a turtle pose broadcast for mystaticturtle to float 1 meter above the ground.

We can now check that the static transform has been published by echoing the tf_static topic If everything is well you should see a single static transform:

ros2 topic echo /tf_static

transforms: 
- header: 
  stamp: 
     sec: 1622908754 
     nanosec: 208515730 
  frame_id: world 
child_frame_id: mystaticturtle 
transform: 
  translation: 
     x: 0.0 
     y: 0.0 
     z: 1.0 
  rotation: 
     x: 0.0 
     y: 0.0 
     z: 0.0 
     w: 1.0

The Proper way to Publish Static Transforms

This tutorial aimed to show how StaticTransformBroadcaster can be used to publish static transforms. In your real development process you shouldnt have to write this code yourself and should use the dedicated tf2_ros tool to do so. tf2_ros provides an executable named static_transform_publisher that can be used either as a commandline tool or a node that you can add to your launchfiles.

The following command publishes a static coordinate transform to tf2 resulting in a 1 meter offset in z and no rotation between the frames world and mystaticturtle. In ROS 2, roll/pitch/yaw refers to rotation in radians about the x/y/z-axis, respectively.

ros2 run tf2_ros \ 
    static_transform_publisher \ 
    --x 0 --y 0 --z 1 \ 
    --yaw 0 --pitch 0 --roll 0 \ 
    --frame-id world --child-frame-id mystaticturtle

The following command publishes the same static coordinate transform to tf2, but using quaternion representation for the rotation.

ros2 run tf2_ros \ 
    static_transform_publisher \ 
    --x 0 --y 0 --z 1 \ 
    --qx 0 --qy 0 --qz 0 --qw 1 \ 
    --frame-id world --child-frame-id mystaticturtle

static_transform_publisher is designed both as a command-line tool for manual use, as well as for use within launch files for setting static transforms. For example:

from launch import LaunchDescription 
from launch_ros.actions import Node 
 
 
def generate_launch_description(): 
   return LaunchDescription([ 
      Node( 
         package='tf2_ros', 
         executable='static_transform_publisher', 
         arguments=[ 
            '--x', '0', '--y', '0', '--z', '1', 
            '--yaw', '0', '--pitch', '0', '--roll', 
            '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle'] 
      ), 
   ])

Note that all arguments except for –frame-id and –child-frame-id are optional; if a particular option isnt specified, then the identity will be assumed.

9.3 Writing a Listener

Writing the Listener Node Lets 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/humble/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

Now open the file called turtle_tf2_listener.py 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.target_frame = self.declare_parameter( 
       'target_frame', 'turtle1').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: 
              t = self.tf_buffer.lookup_transform( 
                 to_frame_rel, 
                 from_frame_rel, 
                 rclpy.time.Time()) 
            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( 
              t.transform.translation.y, 
              t.transform.translation.x) 
 
            scale_forward_speed = 0.5 
            msg.linear.x = scale_forward_speed * math.sqrt( 
              t.transform.translation.x ** 2 + 
              t.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()

Examining the Code Now, lets 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:

• Target frame

• Source frame

• 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.

Adding 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 ).

Add the following line between the 'console_scripts': brackets:

'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',

Updating the Launch Files Open the launch file called turtle_tf2_demo.launch.py in the src/learning\_tf2\_py/launch directory with your text editor, add two new nodes to the launch description, add a launch argument, and add the imports. The resulting file should look like:

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([ 
      Node( 
         package='turtlesim', 
         executable='turtlesim_node', 
         name='sim' 
      ), 
      Node( 
         package='learning_tf2_py', 
         executable='turtle_tf2_broadcaster', 
         name='broadcaster1', 
         parameters=[ 
            {'turtlename': 'turtle1'} 
         ] 
      ), 
      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.

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

Run Now youre 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

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 youll see the second turtle following the first one!

9.4 Adding a Frame

Previously, we’ve recreated the turtle demo by writing a tf2 broadcaster and a tf2 listener. This tutorial will teach you how to add extra fixed and dynamic frames to the transformation tree. In fact, adding a frame in tf2 is very similar to creating the tf2 broadcaster, but this example will show you some additional features of tf2.

For many tasks related to transformations, it is easier to think inside a local frame. For example, it is easiest to reason about laser scan measurements in a frame at the center of the laser scanner. tf2 allows you to define a local frame for each sensor, link, or joint in your system. When transforming from one frame to another, tf2 will take care of all the hidden intermediate frame transformations that are introduced.

Viewing the Tree tf2 builds up a tree structure of frames and, thus, does not allow a closed loop in the frame structure. This means that a frame only has one single parent, but it can have multiple children. Currently, our tf2 tree contains three frames: world, turtle1 and turtle2. The two turtle frames are children of the world frame. If we want to add a new frame to tf2, one of the three existing frames needs to be the parent frame, and the new one will become its child frame.

Writing the Fixed Frame Broadcaster In our turtle example, well add a new frame carrot1, which will be the child of the turtle1. This frame will serve as the goal for the second turtle.

Lets first create the source files. Go to the learning_tf2_py package we created in the previous tutorials. Inside the src/learning_tf2_py/learning_tf2_py directory download the fixed frame broadcaster code by entering the following command:

wget https://raw.githubusercontent.com/ros/geometry_tutorials/humble/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py

Now open the file called fixed_frame_tf2_broadcaster.py.

from geometry_msgs.msg import TransformStamped 
 
import rclpy 
from rclpy.node import Node 
 
from tf2_ros import TransformBroadcaster 
 
 
class FixedFrameBroadcaster(Node): 
 
  def __init__(self): 
     super().__init__('fixed_frame_tf2_broadcaster') 
     self.tf_broadcaster = TransformBroadcaster(self) 
     self.timer = self.create_timer(0.1, self.broadcast_timer_callback) 
 
  def broadcast_timer_callback(self): 
     t = TransformStamped() 
 
     t.header.stamp = self.get_clock().now().to_msg() 
     t.header.frame_id = 'turtle1' 
     t.child_frame_id = 'carrot1' 
     t.transform.translation.x = 0.0 
     t.transform.translation.y = 2.0 
     t.transform.translation.z = 0.0 
     t.transform.rotation.x = 0.0 
     t.transform.rotation.y = 0.0 
     t.transform.rotation.z = 0.0 
     t.transform.rotation.w = 1.0 
 
     self.tf_broadcaster.sendTransform(t) 
 
 
def main(): 
   rclpy.init() 
   node = FixedFrameBroadcaster() 
   try: 
      rclpy.spin(node) 
   except KeyboardInterrupt: 
      pass 
 
   rclpy.shutdown()

The code is very similar to the tf2 broadcaster tutorial example and the only difference is that the transform here does not change over time.

Examining the Code Lets take a look at the key lines in this piece of code. Here we create a new transform, from the parent turtle1 to the new child carrot1. The carrot1 frame is 2 meters offset in y axis in terms of the turtle1 frame.

t = TransformStamped() 
 
t.header.stamp = self.get_clock().now().to_msg() 
t.header.frame_id = 'turtle1' 
t.child_frame_id = 'carrot1' 
t.transform.translation.x = 0.0 
t.transform.translation.y = 2.0 
t.transform.translation.z = 0.0

Adding 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 ).

Add the following line between the 'console_scripts': brackets:

'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',

Writing the Launch File Now lets create a launch file for this example. With your text editor, create a new file called turtle_tf2_fixed_frame_demo.launch.py in the src/learning\_tf2\_py/launch directory, and add the following lines:

from launch import LaunchDescription 
from launch.actions import IncludeLaunchDescription 
from launch.substitutions import PathJoinSubstitution 
from launch_ros.actions import Node 
from launch_ros.substitutions import FindPackageShare 
 
 
def generate_launch_description(): 
   return LaunchDescription([ 
      IncludeLaunchDescription( 
         PathJoinSubstitution([ 
            FindPackageShare('learning_tf2_py'), 'launch', 'turtle_tf2_demo.launch.py']) 
      ), 
      Node( 
         package='learning_tf2_py', 
         executable='fixed_frame_tf2_broadcaster', 
         name='fixed_broadcaster', 
      ), 
   ])

This launch file imports the required packages and then creates a demo_nodes variable that will store nodes that we created in the previous tutorials launch file.

The last part of the code will add our fixed carrot1 frame to the turtlesim world using our fixed_frame_tf2_broadcaster node.

      Node( 
         package='learning_tf2_py', 
         executable='fixed_frame_tf2_broadcaster', 
         name='fixed_broadcaster', 
      ),

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

Run Now you can start the turtle broadcaster demo:

ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo.launch.py

You should notice that the new carrot1 frame appeared in the transformation tree.

If you drive the first turtle around, you should notice that the behavior didnt change from the previous tutorial, even though we added a new frame. Thats because adding an extra frame does not affect the other frames and our listener is still using the previously defined frames.

Therefore if we want our second turtle to follow the carrot instead of the first turtle, we need to change value of the target_frame. This can be done two ways. One way is to pass the target_frame argument to the launch file directly from the console:

ros2 launch learning_tf2_py \ 
     turtle_tf2_fixed_frame_demo.launch.py \ 
     target_frame:=carrot1

The second way is to update the launch file. To do so, open the turtle_tf2_fixed_frame_demo.launch.py file, and add the 'target_frame': 'carrot1' parameter via launch_arguments argument.

def generate_launch_description(): 
   demo_nodes = IncludeLaunchDescription( 
      ..., 
      launch_arguments={'target_frame': 'carrot1'}.items(), 
      )

Now rebuild the package, restart the turtle_tf2_fixed_frame_demo.launch.py, and youll see the second turtle following the carrot instead of the first turtle!

Writing the Dynamic Frame Broadcaster

The extra frame we published in this tutorial is a fixed frame that doesnt change over time in relation to the parent frame. However, if you want to publish a moving frame you can code the broadcaster to change the frame over time. Lets change our carrot1 frame so that it changes relative to turtle1 frame over time. 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 dynamic frame broadcaster code by entering the following command:

wget https://raw.githubusercontent.com/ros/geometry_tutorials/humble/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py

Now open the file called dynamic_frame_tf2_broadcaster.py :

import math 
 
from geometry_msgs.msg import TransformStamped 
 
import rclpy 
from rclpy.node import Node 
 
from tf2_ros import TransformBroadcaster 
 
 
class DynamicFrameBroadcaster(Node): 
 
   def __init__(self): 
      super().__init__('dynamic_frame_tf2_broadcaster') 
      self.tf_broadcaster = TransformBroadcaster(self) 
      self.timer = self.create_timer(0.1, self.broadcast_timer_callback) 
 
   def broadcast_timer_callback(self): 
      seconds, _ = self.get_clock().now().seconds_nanoseconds() 
      x = seconds * math.pi 
 
      t = TransformStamped() 
      t.header.stamp = self.get_clock().now().to_msg() 
      t.header.frame_id = 'turtle1' 
      t.child_frame_id = 'carrot1' 
      t.transform.translation.x = 10 * math.sin(x) 
      t.transform.translation.y = 10 * math.cos(x) 
      t.transform.translation.z = 0.0 
      t.transform.rotation.x = 0.0 
      t.transform.rotation.y = 0.0 
      t.transform.rotation.z = 0.0 
      t.transform.rotation.w = 1.0 
 
      self.tf_broadcaster.sendTransform(t) 
 
 
def main(): 
   rclpy.init() 
   node = DynamicFrameBroadcaster() 
   try: 
      rclpy.spin(node) 
   except KeyboardInterrupt: 
      pass 
 
   rclpy.shutdown()

Examining the Code Instead of a fixed definition of our x and y offsets, we are using the sin() and cos() functions on the current time so that the offset of carrot1 is constantly changing.

seconds, _ = self.get_clock().now().seconds_nanoseconds() 
x = seconds * math.pi 
... 
t.transform.translation.x = 10 * math.sin(x) 
t.transform.translation.y = 10 * math.cos(x)

Adding 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).

Add the following line between the 'console_scripts': brackets:

'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',

Writing the Launch File To test this code, create a new launch file turtle_tf2_dynamic_frame_demo.launch.py in the src/learning\_tf2\_py/launch directory and paste the following code:

from launch import LaunchDescription 
from launch.actions import IncludeLaunchDescription 
from launch.substitutions import PathJoinSubstitution 
from launch_ros.actions import Node 
from launch_ros.substitutions import FindPackageShare 
 
 
def generate_launch_description(): 
   return LaunchDescription([ 
      IncludeLaunchDescription( 
         PathJoinSubstitution([ 
            FindPackageShare('learning_tf2_py'), 'launch', 'turtle_tf2_demo.launch.py']), 
         launch_arguments={'target_frame': 'carrot1'}.items(), 
      ), 
      Node( 
         package='learning_tf2_py', 
         executable='dynamic_frame_tf2_broadcaster', 
         name='dynamic_broadcaster', 
      ), 
   ])

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

Run Now you can start the dynamic frame demo:

ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo.launch.py

You should see that the second turtle is following the carrots position that is constantly changing.

9.5 Writing a Broadcaster

Writing the Broadaster Node Lets 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:

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

Now open the file called turtle_tf2_broadcaster.py 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()

Examining the Code Now, lets 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 {self.turtlename}/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.

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

• Then we need to set the name of the parent frame of the link were creating, in this case world.

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

The handler function for the turtle pose message broadcasts this turtles 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)

Adding an Entry Point To allow the ros2 run command to run your node, you must add the entry point to setup.py

Add the following line between the 'console_scripts': brackets:

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

Writing the Launch File Now create a launch file for this demo. Create a launch folder in the src/learning\_tf2\_py directory. 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'} 
         ] 
      ), 
   ])

Examining 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'} 
         ] 
      ),

Adding Dependencies Navigate one level back to the 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 files 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.

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

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

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

import os 
from glob import glob

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

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

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

colcon build --packages-select learning_tf2_py

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

. install/setup.bash

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 has 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 1714913843.708748879 
- Translation: [4.541, 3.889, 0.000] 
- Rotation: in Quaternion [0.000, 0.000, 0.999, -0.035] 
- Rotation: in RPY (radian) [0.000, -0.000, -3.072] 
- Rotation: in RPY (degree) [0.000, -0.000, -176.013] 
- Matrix: 
 -0.998  0.070  0.000  4.541 
 -0.070 -0.998  0.000  3.889 
  0.000  0.000  1.000  0.000 
  0.000  0.000  0.000  1.000

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.