CMakeLists.txt

cmake_minimum_required(VERSION 3.5)
project(minimal_tf_broadcasters)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

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

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

add_executable(sub src/sub.cpp)
ament_target_dependencies(sub rclcpp geometry_msgs tf2 tf2_ros tf2_geometry_msgs)

install(TARGETS static_pub dynamic_pub sub
  DESTINATION lib/${PROJECT_NAME})

ament_package()

dynamic_pub.cpp

#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class DynamicTfBroadcaster : public rclcpp::Node {
public:
  DynamicTfBroadcaster() : Node("dynamic_tf_equivalent_cpp") {
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    timer_ = this->create_wall_timer(33ms, std::bind(&DynamicTfBroadcaster::publish, this));
  }

private:
  void publish() {
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "foo";
    t.child_frame_id = "bar";
    t.transform.translation.x = 1.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 3.0;

    tf2::Quaternion q; q.setRPY(-3.14, 0.0, 1.57);
    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_broadcaster_->sendTransform(t);
  }

  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DynamicTfBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

dynamic_pub.py

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster

def quat_from_rpy(r, p, y):
    cr, sr = math.cos(r/2.0), math.sin(r/2.0)
    cp, sp = math.cos(p/2.0), math.sin(p/2.0)
    cy, sy = math.cos(y/2.0), math.sin(y/2.0)
    return (
        sr*cp*cy - cr*sp*sy,
        cr*sp*cy + sr*cp*sy,
        cr*cp*sy - sr*sp*cy,
        cr*cp*cy + sr*sp*sy,
    )

class DynTF(Node):
    def __init__(self):
        super().__init__('dynamic_tf_equivalent')
        self.br = TransformBroadcaster(self)
        self.timer = self.create_timer(0.033, self.publish)  # ~30 Hz

    def publish(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'foo'    # parent
        t.child_frame_id = 'bar'     # child
        t.transform.translation.x = 1.0
        t.transform.translation.y = 2.0
        t.transform.translation.z = 3.0
        qx, qy, qz, qw = quat_from_rpy(-3.14, 0.0, 1.57)  # CLI gives yaw,pitch,roll
        t.transform.rotation.x = qx; t.transform.rotation.y = qy
        t.transform.rotation.z = qz; t.transform.rotation.w = qw
        self.br.sendTransform(t)

def main():
    rclpy.init()
    node = DynTF()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

package.xml

<?xml version="1.0"?>
<package format="3">
  <name>minimal_tf_broadcasters</name>
  <version>0.0.1</version>
  <description>Minimal tf2 broadcasters</description>
  <maintainer email="[email protected]">you</maintainer>
  <license>Apache-2.0</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>geometry_msgs</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
</package>

ros2-tf2-conventions.md

TF2 ROS2

This describes how to relate the tf2 transforms conventions with your standard T_X_to_Y notations, for ros2 (humble and onwards)

Concrete example: Say foo is the parent frame / fixed frame. You want to go to bar. You do these transformations in that order:

• translate 1m in X, 2m in Y, 3m in Z
• rotate 90deg about Z, ie, yaw = 90deg
• rotate -180deg about X, ie, roll = -180deg

Hand-verified ground-truth solution:

T_foo_to_bar = [
    [0, 1, 0, -2],
    [1, 0, 0, -1],
    [0, 0, -1, 3],
    [0, 0, 0, 1]
  ]

Below, we'll reproduce this exactly using tf2 commandline, python3 and c++ apis.

tf2 commandline equivalent for static transform

ros2 run tf2_ros static_transform_publisher 1 2 3 1.57 0.0 -3.14 foo bar: this applies the translation (x=1, y=2, z=3), and then the body-fixed rotations (yaw, pitch, roll) in that sequence, from parent frame (foo) to child frame (bar) Output:

[WARN] [1760894974.136992725] []: Old-style arguments are deprecated; see --help for new-style arguments
[INFO] [1760894974.159561405] [static_transform_publisher_qbaLLikc4FSCUcWX]: Spinning until stopped - publishing transform
translation: ('1.000000', '2.000000', '3.000000')
rotation: ('-0.707388', '-0.706825', '0.000563', '0.000563')
from 'foo' to 'bar'

ros2 run tf2_ros tf2_echo bar foo -p 1: echo the transform from foo to bar (same as above essentially). Note the order of frames args is reversed. Also, the output of this command lists Matrix, which is what you should focus on, the rest of the entries I think are inverted, as in, if you do args as foo bar, it might show what you would expect Output:

[INFO] [1760895230.843096430] [tf2_echo]: Waiting for transform bar ->  foo: Invalid frame ID "bar" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [-2.0, -1.0, 3.0]
- Rotation: in Quaternion (xyzw) [0.7, 0.7, -0.0, 0.0]
- Rotation: in RPY (radian) [3.1, 0.0, 1.6]
- Rotation: in RPY (degree) [180.0, 0.1, 90.0]
- Matrix:
    0.0    1.0    0.0   -2.0
    1.0   -0.0   -0.0   -1.0
   -0.0    0.0   -1.0    3.0
    0.0    0.0    0.0    1.0

---

python3

publishing / broadcasting

• static transforms api: static_pub.py
• dynamic transforms api: dynamic_pub.py

subscribing / listening

• sub.py

---

c++

publishing / broadcasting

• static transforms api: static_pub.cpp
• dynamic transforms api: dynamic_pub.cpp

subscribing / listening

• sub.cpp

---

static_pub.cpp

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

int main(int argc, char* argv[]) {
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("static_tf_equivalent_cpp");

  tf2_ros::StaticTransformBroadcaster static_broadcaster(node);

  geometry_msgs::msg::TransformStamped t;
  t.header.stamp = node->get_clock()->now();
  t.header.frame_id = "foo";
  t.child_frame_id = "bar";
  t.transform.translation.x = 1.0;
  t.transform.translation.y = 2.0;
  t.transform.translation.z = 3.0;

  tf2::Quaternion q; q.setRPY(-3.14, 0.0, 1.57); // roll,pitch,yaw
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();

  static_broadcaster.sendTransform(t);
  rclcpp::spin(node);  // keep alive so late-joiners see /tf_static
  rclcpp::shutdown();
  return 0;
}

static_pub.py

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

def quat_from_rpy(roll, pitch, yaw):
    cr, sr = math.cos(roll/2.0), math.sin(roll/2.0)
    cp, sp = math.cos(pitch/2.0), math.sin(pitch/2.0)
    cy, sy = math.cos(yaw/2.0), math.sin(yaw/2.0)
    return (
        sr*cp*cy - cr*sp*sy,  # x
        cr*sp*cy + sr*cp*sy,  # y
        cr*cp*sy - sr*sp*cy,  # z
        cr*cp*cy + sr*sp*sy,  # w
    )

def main():
    rclpy.init()
    node = Node('static_tf_equivalent')
    br = StaticTransformBroadcaster(node)

    t = TransformStamped()
    t.header.stamp = node.get_clock().now().to_msg()
    t.header.frame_id = 'foo'           # parent
    t.child_frame_id = 'bar'            # child
    t.transform.translation.x = 1.0
    t.transform.translation.y = 2.0
    t.transform.translation.z = 3.0

    # CLI uses (x y z YAW PITCH ROLL); quaternion expects (roll, pitch, yaw)
    qx, qy, qz, qw = quat_from_rpy(-3.14, 0.0, 1.57)
    t.transform.rotation.x = qx
    t.transform.rotation.y = qy
    t.transform.rotation.z = qz
    t.transform.rotation.w = qw

    br.sendTransform(t)
    rclpy.spin(node)  # keep node alive so late-joiners receive /tf_static
    rclpy.shutdown()

if __name__ == '__main__':
    main()

sub.cpp

#include <chrono>
#include <iostream>
#include <iomanip>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"

using namespace std::chrono_literals;

class TFPosePrinter : public rclcpp::Node {
public:
  TFPosePrinter()
  : Node("tf_pose_printer_cpp"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {
    timer_ = this->create_wall_timer(100ms, std::bind(&TFPosePrinter::tick, this));
  }

private:
  void tick() {
    if (done_) return;

    geometry_msgs::msg::PoseStamped pose_foo;
    pose_foo.header.frame_id = "foo";
    pose_foo.header.stamp = this->get_clock()->now();
    pose_foo.pose.position.x = 0.4;
    pose_foo.pose.position.y = -0.1;
    pose_foo.pose.position.z = 0.2;
    pose_foo.pose.orientation.w = 1.0; // identity

    try {
      // Transform at the pose's own timestamp
      auto pose_bar = tf_buffer_.transform(pose_foo, "bar", 1s);
      auto tf_bar_foo = tf_buffer_.lookupTransform("bar", "foo", rclcpp::Time(pose_foo.header.stamp), 1s);

      printResults(pose_foo, pose_bar, tf_bar_foo);
      done_ = true;
      rclcpp::shutdown();
    } catch (const tf2::TransformException & ex) {
      // Not available yet; wait for next tick
    }
  }

  static void printResults(const geometry_msgs::msg::PoseStamped & in,
                           const geometry_msgs::msg::PoseStamped & out,
                           const geometry_msgs::msg::TransformStamped & tf) {
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "Input pose (frame \"foo\"): ["
              << in.pose.position.x << ", " << in.pose.position.y << ", " << in.pose.position.z << "]\n";
    std::cout << "Transformed pose (frame \"bar\"): ["
              << out.pose.position.x << ", " << out.pose.position.y << ", " << out.pose.position.z << "]\n";

    const auto & tr = tf.transform.translation;
    const auto & qr = tf.transform.rotation;
    tf2::Quaternion q(qr.x, qr.y, qr.z, qr.w);
    tf2::Matrix3x3 R(q);
    auto r0 = R.getRow(0), r1 = R.getRow(1), r2 = R.getRow(2);

    std::cout << "foo -> bar as 4x4 (T_bar_foo):\n";
    std::cout << "  " << std::setprecision(1)
              << r0.x() << " " << r0.y() << " " << r0.z() << " " << tr.x << "\n"
              << "  " << r1.x() << " " << r1.y() << " " << r1.z() << " " << tr.y << "\n"
              << "  " << r2.x() << " " << r2.y() << " " << r2.z() << " " << tr.z << "\n"
              << "  0 0 0 1\n";
  }

  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  rclcpp::TimerBase::SharedPtr timer_;
  bool done_ = false;
};

int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<TFPosePrinter>());
  return 0;
}

sub.py

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from rclpy.time import Time
from geometry_msgs.msg import PoseStamped
from tf2_ros import Buffer, TransformListener
import tf2_geometry_msgs

def quat_to_rot(qx, qy, qz, qw):
    xx, yy, zz = qx*qx, qy*qy, qz*qz
    xy, xz, yz = qx*qy, qx*qz, qy*qz
    wx, wy, wz = qw*qx, qw*qy, qw*qz
    r00 = 1.0 - 2.0*(yy + zz); r01 = 2.0*(xy - wz);     r02 = 2.0*(xz + wy)
    r10 = 2.0*(xy + wz);       r11 = 1.0 - 2.0*(xx+zz); r12 = 2.0*(yz - wx)
    r20 = 2.0*(xz - wy);       r21 = 2.0*(yz + wx);     r22 = 1.0 - 2.0*(xx+yy)
    return [[r00, r01, r02], [r10, r11, r12], [r20, r21, r22]]

def mat4_from_transform(tf):
    tx, ty, tz = tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z
    qx, qy, qz, qw = tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z, tf.transform.rotation.w
    R = quat_to_rot(qx, qy, qz, qw)
    return [
        [R[0][0], R[0][1], R[0][2], tx],
        [R[1][0], R[1][1], R[1][2], ty],
        [R[2][0], R[2][1], R[2][2], tz],
        [0.0, 0.0, 0.0, 1.0],
    ]

def fmt_mat4(m):
    return '\n'.join('  ' + ' '.join(f'{v: .1f}' for v in row) for row in m)

class TFPosePrinter(Node):
    def __init__(self):
        super().__init__('tf_pose_printer')
        self.buf = Buffer()
        self.listener = TransformListener(self.buf, self)
        self.done = False
        self.create_timer(0.1, self.tick)

    def tick(self):
        if self.done:
            return
        # Dummy pose in frame 'foo' at "now"
        pose_foo = PoseStamped()
        pose_foo.header.frame_id = 'foo'
        pose_foo.header.stamp = self.get_clock().now().to_msg()
        pose_foo.pose.position.x = 0.4
        pose_foo.pose.position.y = -0.1
        pose_foo.pose.position.z = 0.2
        pose_foo.pose.orientation.w = 1.0  # identity

        t = rclpy.time.Time.from_msg(pose_foo.header.stamp)

        # Wait for foo->bar at the pose's timestamp, then transform
        if not self.buf.can_transform('bar', 'foo', t, timeout=Duration(seconds=2.0)):
            return
        pose_bar = self.buf.transform(pose_foo, 'bar', timeout=Duration(seconds=1.0))
        tf_bar_foo = self.buf.lookup_transform('bar', 'foo', t, timeout=Duration(seconds=1.0))

        self.get_logger().info(f'Input pose (frame "foo"): [{pose_foo.pose.position.x:.3f}, '
                               f'{pose_foo.pose.position.y:.3f}, {pose_foo.pose.position.z:.3f}]')
        self.get_logger().info(f'Transformed pose (frame "bar"): [{pose_bar.pose.position.x:.3f}, '
                               f'{pose_bar.pose.position.y:.3f}, {pose_bar.pose.position.z:.3f}]')
        self.get_logger().info('foo -> bar as 4x4 (T_bar_foo):\n' + fmt_mat4(mat4_from_transform(tf_bar_foo)))

        self.done = True
        rclpy.shutdown()

def main():
    rclpy.init()
    rclpy.spin(TFPosePrinter())

if __name__ == '__main__':
    main()