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

Writing a tf2 static broadcaster (C++)

Goal: Learn how to broadcast static coordinate frames to tf2.

Tutorial level: Intermediate

Time: 15 minutes

Background

Publishing static transforms is useful to define the relationship between a robot base and its sensors or non-moving parts. For example, it is easiest to reason about laser scan measurements in a frame at the center of the laser scanner.

This is a standalone tutorial covering the basics of static transforms, which consists of two 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 commandline static_transform_publisher executable tool in tf2_ros.

In the next two tutorials we will write the code to reproduce the demo from the Introduction to tf2 tutorial. After that, the following tutorials focus on extending the demo with more advanced tf2 features.

Prerequisites

In previous tutorials, you learned how to create a workspace and create a package.

Tasks

1 Create a package

First we will create a package that will be used for this tutorial and the following ones. The package called learning_tf2_cpp will depend on rclcpp, tf2, tf2_ros, geometry_msgs, and turtlesim. Code for this tutorial is stored here.

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

ros2 pkg create --build-type ament_cmake learning_tf2_cpp

Your terminal will return a message verifying the creation of your package learning_tf2_cpp and all its necessary files and folders.

2 Write the static broadcaster node

Let’s first create the source files. Inside the src/learning_tf2_cpp/src directory download the example static broadcaster code by entering the following command:

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

Open the file using your preferred text editor.

#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <memory>

using std::placeholders::_1;

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    rclcpp::Time now = this->get_clock()->now();
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_publisher_->sendTransform(t);
  }
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 Examine the code

Now let’s look at the code that is relevant to publishing the static turtle pose to tf2. The first lines include the required header files. First we include geometry_msgs/msg/transform_stamped.hpp to access the TransformStamped message type, which we will publish to the transformation tree.

#include <geometry_msgs/msg/transform_stamped.hpp>

Afterward, rclcpp is included so its rclcpp::Node class can be used.

#include <rclcpp/rclcpp.hpp>

tf2::Quaternion is a class for a quaternion that provides convenient functions for converting Euler angles to quaternions and vice versa. We also include tf2_ros/static_transform_broadcaster.h to use the StaticTransformBroadcaster to make the publishing of static transforms easy.

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>

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.

tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

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

  1. We need to give the transform being published a timestamp and we’ll just stamp it with the current time, this->get_clock()->now()

  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 frame of the link we’re creating

rclcpp::Time now = this->get_clock()->now();
geometry_msgs::msg::TransformStamped t;

t.header.stamp = now;
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 = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

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

tf_publisher_->sendTransform(t);

2.2 Add dependencies

Navigate one level back to the src/learning_tf2_cpp directory, where the CMakeLists.txt and package.xml files have been created for you.

Open package.xml with your text editor.

As mentioned in the Creating your first ROS 2 package tutorial, make sure to fill in the <description>, <maintainer> and <license> tags:

<description>Learning tf2 with rclcpp</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 node’s include statements:

<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>

This declares the required geometry_msgs, rclcpp, tf2, tf2_ros, and turtlesim dependencies when its code is built and executed.

Make sure to save the file.

2.3 CMakeLists.txt

Now open the CMakeLists.txt file. Below the existing dependency find_package(ament_cmake REQUIRED), add the lines:

find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)

After that, add the executable and name it static_turtle_tf2_broadcaster, which you’ll use later with ros2 run.

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
   turtlesim
)

Finally, add the install(TARGETS…) section so ros2 run can find your executable:

install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

3 Build and run

It’s 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 rolling -y

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

colcon build --packages-select learning_tf2_cpp

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

. install/setup.bash

Now run the static_turtle_tf2_broadcaster node:

ros2 run learning_tf2_cpp 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

ros2 topic echo /tf_static

If everything went well you should see a single static transform

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 shouldn’t 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.

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and roll/pitch/yaw in radians. In our case, roll/pitch/yaw refers to rotation about the x/y/z-axis, respectively.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and quaternion.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

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 isn’t specified, then the identity will be assumed.

Summary

In this tutorial you learned how static transforms are useful to define static relationships between frames, like mystaticturtle in relation to the world frame. In addition, you learned how static transforms can be useful for understanding sensor data, such as from laser scanners, by relating the data to a common coordinate frame. Finally, you wrote your own node to publish static transforms to tf2 and learned how to publish required static transformations using static_transform_publisher executable and launch files.