navigationros2tutorialamrtf2urdfrviz2cpppython

ROS 2 A to Z (P4): TF2, URDF and RViz2

Describe your robot's shape with URDF, track coordinate relationships between its parts with TF2, and visualize it all in RViz2 — C++ and Python examples (Jazzy, June 2026).

Nguyen Anh TuanFebruary 20, 20267 min readUpdated: Jun 14, 2026
ROS 2 A to Z (P4): TF2, URDF and RViz2

A robot is a collection of moving parts: wheels, arms, a camera, a LiDAR. The core question is always: where is the object the camera sees relative to the robot center? Relative to a wheel? To answer it, ROS 2 needs to know the coordinate relationships between every part. Those are the three tools of this article: URDF (describe shape), TF2 (track coordinate relationships), and RViz2 (see it all).

This article continues from Part 3: Parameters, Launch, Lifecycle.

Version (June 2026): ROS 2 Jazzy (Ubuntu 24.04). Examples in both C++ and Python. RViz2 is the main visualization tool. On Humble, swap jazzyhumble.

Coordinate frames and a robot model in RViz2
Coordinate frames and a robot model in RViz2

1. TF2 — the coordinate-frame tree

Each robot part has a frame (its own coordinate system): base_link (robot center), laser (LiDAR), camera_link, left_wheel… TF2 maintains a transform tree describing the position + orientation (translation + rotation) of each child frame relative to its parent, in real time.

            map
             │
            odom
             │
         base_link
        ┌────┼─────────┐
     laser  camera   left_wheel

Thanks to this tree, you ask "where is a point in the laser frame, expressed in map?" and TF2 computes the chain of transforms — you don't multiply matrices by hand.

Static transform — a rigidly attached frame

A LiDAR bolted to the body → the base_link → laser relationship never changes. That's a static transform.

# Quickest way: publish a static TF from the CLI
# args: x y z yaw pitch roll  parent_frame  child_frame
ros2 run tf2_ros static_transform_publisher \
  0.1 0 0.2 0 0 0 base_link laser

In a launch file (the practical way):

from launch_ros.actions import Node

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    arguments=['0.1', '0', '0.2', '0', '0', '0', 'base_link', 'laser'],
)

Dynamic transform — a moving frame (code)

The odom → base_link relationship changes constantly as the robot moves → you broadcast the transform in code.

Python:

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


class OdomBroadcaster(Node):
    def __init__(self):
        super().__init__('odom_broadcaster')
        self.br = TransformBroadcaster(self)
        self.timer = self.create_timer(0.05, self.tick)  # 20 Hz
        self.x = 0.0

    def tick(self):
        self.x += 0.01  # simulate the robot moving forward
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'odom'
        t.child_frame_id = 'base_link'
        t.transform.translation.x = self.x
        # Quaternion for yaw = 0
        t.transform.rotation.w = 1.0
        self.br.sendTransform(t)

C++:

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"

class OdomBroadcaster : public rclcpp::Node
{
public:
  OdomBroadcaster() : Node("odom_broadcaster")
  {
    br_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    timer_ = create_wall_timer(std::chrono::milliseconds(50),
                               std::bind(&OdomBroadcaster::tick, this));
  }
private:
  void tick()
  {
    x_ += 0.01;
    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now();
    t.header.frame_id = "odom";
    t.child_frame_id = "base_link";
    t.transform.translation.x = x_;
    t.transform.rotation.w = 1.0;
    br_->sendTransform(t);
  }
  std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
  rclcpp::TimerBase::SharedPtr timer_;
  double x_ = 0.0;
};

Look up a transform — frame A relative to frame B

Python (lookup):

from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

class FrameLookup(Node):
    def __init__(self):
        super().__init__('frame_lookup')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.create_timer(1.0, self.lookup)

    def lookup(self):
        try:
            tf = self.buffer.lookup_transform('map', 'laser', rclpy.time.Time())
            self.get_logger().info(
                f'laser in map: x={tf.transform.translation.x:.2f}')
        except TransformException as e:
            self.get_logger().warn(f'No transform yet: {e}')

TF2 debug tools

# Draw the frame tree to a PDF
ros2 run tf2_tools view_frames

# Continuously print the transform between two frames
ros2 run tf2_ros tf2_echo base_link laser

Classic error: "lookup would require extrapolation into the future." It means you asked for a transform at a time that has no data yet. Use rclpy.time.Time() (empty = "latest") instead of a specific timestamp while learning.

2. URDF — describe the robot's shape

URDF (Unified Robot Description Format) is an XML file describing a robot as links (rigid parts) connected by joints.

<!-- robot.urdf -->
<?xml version="1.0"?>
<robot name="my_robot">

  <!-- Robot body -->
  <link name="base_link">
    <visual>
      <geometry><box size="0.3 0.2 0.1"/></geometry>
      <material name="blue"><color rgba="0 0 0.8 1"/></material>
    </visual>
  </link>

  <!-- LiDAR -->
  <link name="laser">
    <visual>
      <geometry><cylinder radius="0.03" length="0.04"/></geometry>
    </visual>
  </link>

  <!-- Joint mounting the LiDAR on the body (fixed) -->
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser"/>
    <origin xyz="0.1 0 0.1" rpy="0 0 0"/>
  </joint>

</robot>

Important joint types:

Type Meaning
fixed Rigid, doesn't move (LiDAR, camera)
continuous Rotates infinitely (wheels)
revolute Rotates within angle limits (arm joints)
prismatic Slides linearly (gripper, lift)

Xacro — cleaner URDF

Real URDF is long and repetitive. Xacro adds variables and macros:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">
  <xacro:property name="wheel_radius" value="0.05"/>

  <xacro:macro name="wheel" params="prefix y_pos">
    <link name="${prefix}_wheel">
      <visual><geometry>
        <cylinder radius="${wheel_radius}" length="0.04"/>
      </geometry></visual>
    </link>
    <joint name="${prefix}_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_wheel"/>
      <origin xyz="0 ${y_pos} 0" rpy="${-pi/2} 0 0"/>
      <axis xyz="0 0 1"/>
    </joint>
  </xacro:macro>

  <xacro:wheel prefix="left"  y_pos="0.12"/>
  <xacro:wheel prefix="right" y_pos="-0.12"/>
</robot>

robot_state_publisher — turn URDF into TF2

This is the key bridge: robot_state_publisher reads the URDF + joint states (/joint_states) and automatically publishes all of TF2 for every link. You don't write a broadcaster per frame.

# launch/display.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def generate_launch_description():
    pkg = get_package_share_directory('my_robot_desc')
    urdf = os.path.join(pkg, 'urdf', 'robot.urdf.xacro')
    robot_desc = xacro.process_file(urdf).toxml()

    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': robot_desc}],
        ),
        # GUI sliders to rotate joints (for testing only)
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
        ),
        Node(package='rviz2', executable='rviz2'),
    ])

3. RViz2 — see the robot and its data

RViz2 is ROS 2's 3D visualization tool. After running the launch above:

ros2 launch my_robot_desc display.launch.py

In RViz2:

  1. Set Fixed Frame = base_link (top-left).
  2. Click Add → add a RobotModel display → see the robot built from URDF.
  3. AddTF → see the coordinate axes of every frame.
  4. Drag the sliders in joint_state_publisher_gui → the wheels rotate in RViz2.

Other useful displays: LaserScan (/scan from LiDAR), PointCloud2, Image (camera), Path (Nav2's path). This is how you "see" what the robot is thinking — invaluable when debugging.

Tip: Save your RViz2 config (File → Save Config As) to a .rviz file, then auto-load it in launch: arguments=['-d', rviz_config_path]. No more re-adding displays each time.

Common pitfalls

  1. RobotModel doesn't show — wrong Fixed Frame, or robot_state_publisher isn't running, or the URDF has a link not connected into the joint tree.
  2. TF is "disconnected" — a transform in the chain is missing (e.g. no odom → base_link). Run ros2 run tf2_tools view_frames to see where the tree breaks.
  3. Xacro parse error — missing xmlns:xacro, or using ${} without declaring the property.
  4. Wheel rotates on the wrong axis — wrong axis or rpy in the joint. Build each link in RViz2 to check visually.

Summary

  • URDF/Xacro describe the robot's shape with links + joints.
  • robot_state_publisher turns the URDF into a TF2 tree automatically.
  • TF2 answers every "where is frame A relative to frame B" question.
  • RViz2 lets you see the robot, the frames, and sensor data.

With URDF and TF2 in hand, we're ready to drive real hardware. Part 5: ros2_control will use this very URDF to declare a hardware interface and drive motors.


NT

Nguyễn Anh Tuấn

Robotics & AI Engineer. Building VnRobo — sharing knowledge about robot learning, VLA models, and automation.

Khám phá VnRobo

Related Posts

ROS 2 từ A đến Z (P3): Parameters, Launch, Lifecycle
navigation

ROS 2 từ A đến Z (P3): Parameters, Launch, Lifecycle

2/6/20267 min read
NT
ROS 2 từ A đến Z (P6): Nav2 — Robot tự hành
navigation

ROS 2 từ A đến Z (P6): Nav2 — Robot tự hành

3/20/202611 min read
NT
ROS 2 từ A đến Z (Phần 2): Topics, Services và Actions
navigation

ROS 2 từ A đến Z (Phần 2): Topics, Services và Actions

1/21/202610 min read
NT