CSE276A - ROS2 TF

Introduction

There are many coordinate systems in the environment (e.g., robot frame, world frame, camera frame, arm frame, landmark frame). If we can get the transformation from camera to a landmark, then where is the landmark relative to the robot? The ROS2 tf system can help us do the transformation.

tf2

tf2 stores all frames in a tree structure.

• Each frame has only one parent frame.
• A parent frame can have many children frames.

This tree structure ensures there is always a single path between any two frames. If a frame is not connected to the main tree, tf2 cannot transform to it.

Static Transform Broadcaster

This node publishes an unchanging transform. This is perfect for fixed relationships. This node publish transforms to /tf_static topic.

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
import tf_transformations

class StaticFramePublisher(Node):
    """
    This node publishes a static transform between 'base_link' and 'camera_link'.
    """
    def __init__(self):
        super().__init__('static_transform_broadcaster_node')

        # 1. Initialize the StaticTransformBroadcaster
        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # 2. Create the TransformStamped message
        static_transform = TransformStamped()
        static_transform.header.stamp = self.get_clock().now().to_msg()
        static_transform.header.frame_id = 'base_link'  # Parent frame
        static_transform.child_frame_id = 'camera_link' # Child frame

        # 3. Set the translation
        static_transform.transform.translation.x = 0.5
        static_transform.transform.translation.y = 0.0
        static_transform.transform.translation.z = 0.2

        # 4. Set the rotation
        q = tf_transformations.quaternion_from_euler(0, 0, 0) # default order: (roll, pitch, yaw)
        static_transform.transform.rotation.x = q[0]
        static_transform.transform.rotation.y = q[1]
        static_transform.transform.rotation.z = q[2]
        static_transform.transform.rotation.w = q[3]

        # 5. Publish the transform
        self.tf_static_broadcaster.sendTransform(static_transform)
        self.get_logger().info('Published static transform: base_link -> camera_link')

Dynamic Transform Broadcaster

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
import tf_transformations
import math

class DynamicFrameBroadcaster(Node):
    """
    This node publishes a dynamic transform from 'odom' to 'base_link'.
    """
    def __init__(self):
        super().__init__('dynamic_transform_broadcaster_node')

        # 1. Initialize the TransformBroadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # 2. Create a timer to call the publish_callback function
        self.timer = self.create_timer(0.1, self.publish_callback) # 10 Hz
        self.get_logger().info('Publishing dynamic transform: odom -> base_link')

    def publish_callback(self):
        current_time = self.get_clock().now()
        
        seconds, _ = self.get_clock().now().seconds_nanoseconds()
        
        t = TransformStamped()

        t.header.stamp = current_time.to_msg()
        t.header.frame_id = 'odom'
        t.child_frame_id = 'base_link'

        t.transform.translation.x = math.sin(seconds)
        t.transform.translation.y = math.cos(seconds)
        t.transform.translation.z = 0.0

        q = tf_transformations.quaternion_from_euler(0, 0, -seconds) # (roll, pitch, yaw)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.tf_broadcaster.sendTransform(t)

Transform Listener

This node listens to the /tf and /tf_static topics (via the buffer) and can look up any transform in the tree.

• The tf2::Buffer is is the “brain” of the operation. When you ask for odom -> camera_link, it looks at its data and figures out the path, then performs all the mathematical calculations to get the final transform.
• The TransformListener automatically subscribe to the /tf and /tf_static topics and feed all the incoming transform messages directly into the tf2::Buffer.

import rclpy
from rclpy.node import Node
from rclpy.duration import Duration
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

class FrameListener(Node):
    """
    This node listens to the TF tree and looks up the transform
    from 'odom' (target) to 'camera_link' (source).
    """
    def __init__(self):
        super().__init__('tf_listener_node')

        self.target_frame = 'odom'
        self.source_frame = 'camera_link'

        # The Buffer stores incoming transforms
        self.tf_buffer = Buffer()
        # The Listener subscribes to /tf and /tf_static and fills the buffer
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.timer = self.create_timer(1.0, self.lookup_callback) # 1 Hz

    def lookup_callback(self):
        target = self.target_frame
        source = self.source_frame

        try:
            # Look up the transform
            trans = self.tf_buffer.lookup_transform(
                target,
                source,
                rclpy.time.Time(), # this parameter means finding the newest transform in the buffer
                timeout=Duration(seconds=0.5)) # Wait up to 0.5s for the transform, if time exceeds, throw exception

            self.get_logger().info(f'  Translation: [x: {trans.transform.translation.x:.2f}, ' +
                                  f'y: {trans.transform.translation.y:.2f}, ' +
                                  f'z: {trans.transform.translation.z:.2f}]')
            self.get_logger().info(f'  Rotation: [x: {trans.transform.rotation.x:.2f}, ' +
                                  f'y: {trans.transform.rotation.y:.2f}, ' +
                                  f'z: {trans.transform.rotation.z:.2f}, ' +
                                  f'w: {trans.transform.rotation.w:.2f}]')

        except TransformException as ex:
            # Handle exceptions (e.g., transform not available)
            self.get_logger().warn(
                f'Could not get transform from {source} to {target}: {ex}')

TransformStamped Message

TransformStamped is the standard ROS message used to communicate a single transformation. It’s not a matrix itself, but it contains the data needed to build one.

• header.stamp: When this transform was measured
• header.frame_id: The parent frame
• child_frame_id: The child frame
• transform.translation: The $\mathbf{t}$ vector
• transform.rotation: The $\mathbf{R}$ matrix represented as a quaternion

tf_transformations Package

This package helps us do the calculations related to transformations.

• translation_matrix([x, y, z]): Creates a 4x4 translation-only matrix.
• quaternion_matrix([x, y, z, w]): Creates a 4x4 rotation-only matrix from a quaternion.
• concatenate_matrices(matrix1, matrix2, ...): Multiplies (chains) matrices.
• inverse_matrix(matrix): Calculates the inverse of a 4x4 matrix (${}^B T_{A}$ to ${}^{A} T_B$).
• translation_from_matrix(matrix): Extracts the translation vector $[t_x, t_y, t_z]$ from a 4x4 matrix.
• quaternion_from_matrix(matrix): Extracts the rotation part as a quaternion $[x, y, z, w]$.
• (roll, pitch, yaw) = tf_transformations.euler_from_quaternion([x, y, z, w], axes='sxyz'): Converts a quaternion to Euler angles.

Suppose we have a TransformStamped message and we want to build a homogeneous from it:

trans = Tstamped.transform.translation
rot = Tstamped.transform.rotation
Tmatrix = tf_transformations.concatenate_matrices(
    tf_transformations.translation_matrix([trans.x, trans.y, trans.z]),
    tf_transformations.quaternion_matrix([rot.x, rot.y, rot.z, rot.w])
)

When we apply a homogeneous transformation to a point, $p_A = {}^A T_B \cdot p_B$, where

$${}^A T_B = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}$$

It’s equivalent to $p_A = R \cdot p_B + t$, which means, we first rotate $p$, then apply translation. This is equivalent to $p_A = (T_{translation} \cdot T_{rotation}) \cdot p_B$, as a result

$${}^A T_B = T_{translation} \cdot T_{rotation}$$

That’s why in the above code, we first concatenate translation then rotation.