This tutorial demonstrates how to model a walking robot using URDF, publish its state as tf2 messages, and visualize the simulation in Rviz. We'll create a URDF model describing the robot's assembly, implement a node to simulate motion and publish joint states and transforms, and use robot_state_publisher to broadcast the complete robot state to /tf2.

Prerequisites

• rviz2 installed
• ROS 2 environment sourced in each terminal

source /opt/ros/jazzy/setup.bash
echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc

Package Creaiton

Create a workspace and package:

mkdir -p second_ros2_ws/src
cd second_ros2_ws/src
ros2 pkg create --build-type ament_python --license Apache-2.0 urdf_tutorial_r2d2 --dependencies rclpy
cd urdf_tutorial_r2d2

URDF File Setup

Create a directory for assets and download required files:

mkdir -p urdf

Download the URDF file and save it as second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.urdf.xml. Download the Rviz configuration file and save it as second_ros2_ws/src/urdf_tutorial_r2d2/urdf/r2d2.rviz.

State Publisher Implementation

Create second_ros2_ws/src/urdf_tutorial_r2d2/urdf_tutorial_r2d2/state_publisher.py with the following code:

import math
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped

class RobotStatePublisher(Node):
    def __init__(self):
        rclpy.init()
        super().__init__('robot_state_publisher')
        
        qos = QoSProfile(depth=10)
        self.joint_publisher = self.create_publisher(JointState, 'joint_states', qos)
        self.transform_broadcaster = TransformBroadcaster(self, qos=qos)
        self.get_logger().info(f"{self.get_name()} initialized")
        
        rad_per_deg = math.pi / 180.0
        update_rate = self.create_rate(30)
        
        tilt_angle = 0.0
        tilt_delta = rad_per_deg
        rotation_angle = 0.0
        base_angle = 0.0
        height_val = 0.0
        height_delta = 0.005
        
        transform_msg = TransformStamped()
        transform_msg.header.frame_id = 'odom'
        transform_msg.child_frame_id = 'axis'
        joint_msg = JointState()
        
        try:
            while rclpy.ok():
                rclpy.spin_once(self)
                
                current_time = self.get_clock().now()
                joint_msg.header.stamp = current_time.to_msg()
                joint_msg.name = ['rotation_joint', 'tilt_joint', 'height_joint']
                joint_msg.position = [rotation_angle, tilt_angle, height_val]
                
                transform_msg.header.stamp = current_time.to_msg()
                transform_msg.transform.translation.x = math.cos(base_angle) * 2
                transform_msg.transform.translation.y = math.sin(base_angle) * 2
                transform_msg.transform.translation.z = 0.7
                transform_msg.transform.rotation = self.euler_to_quaternion(0, 0, base_angle + math.pi/2)
                
                self.joint_publisher.publish(joint_msg)
                self.transform_broadcaster.sendTransform(transform_msg)
                
                tilt_angle += tilt_delta
                if tilt_angle < -0.5 or tilt_angle > 0.0:
                    tilt_delta *= -1
                
                height_val += height_delta
                if height_val > 0.2 or height_val < 0.0:
                    height_delta *= -1
                
                rotation_angle += rad_per_deg
                base_angle += rad_per_deg / 4
                
                update_rate.sleep()
        except KeyboardInterrupt:
            pass
    
    def euler_to_quaternion(self, roll, pitch, yaw):
        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        return Quaternion(x=qx, y=qy, z=qz, w=qw)

def main():
    node = RobotStatePublisher()

if __name__ == '__main__':
    main()

Launch File Configuration

Create second_ros2_ws/src/urdf_tutorial_r2d2/launch/demo_launch.py:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    sim_time_param = LaunchConfiguration('use_sim_time', default='false')
    
    urdf_filename = 'r2d2.urdf.xml'
    urdf_path = os.path.join(
        get_package_share_directory('urdf_tutorial_r2d2'),
        urdf_filename)
    
    with open(urdf_path, 'r') as file:
        robot_description = file.read()
    
    return LaunchDescription([
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation clock when true'),
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'use_sim_time': sim_time_param, 'robot_description': robot_description}],
            arguments=[urdf_path]),
        Node(
            package='urdf_tutorial_r2d2',
            executable='state_publisher',
            name='state_publisher',
            output='screen'),
    ])

Package Configuration

Edit second_ros2_ws/src/urdf_tutorial_r2d2/setup.py:

import os
from glob import glob
from setuptools import setup
from setuptools import find_packages

package_name = 'urdf_tutorial_r2d2'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
        (os.path.join('share', package_name), glob('urdf/*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='user',
    maintainer_email='user@example.com',
    description='URDF tutorial for R2D2 robot',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'state_publisher = urdf_tutorial_r2d2.state_publisher:main'
        ],
    },
)

Build and Installation

Build the package:

cd second_ros2_ws
colcon build --symlink-install --packages-select urdf_tutorial_r2d2

Source the installation:

source install/setup.bash

Visualization

Launch the simulation:

ros2 launch urdf_tutorial_r2d2 demo_launch.py

In a new terminal, start Rviz with the configuration:

rviz2 -d second_ros2_ws/install/urdf_tutorial_r2d2/share/urdf_tutorial_r2d2/r2d2.rviz