Chapter 1: ROS 2 Fundamentals for Humanoid Robotics

Learning Objectives

By the end of this chapter, you will be able to:

• Explain the architecture of ROS 2 and why it was designed
• Create and manage ROS 2 nodes in Python and C++
• Understand the publish-subscribe pattern using topics
• Implement request-response communication with services
• Use actions for long-running tasks with feedback
• Design a simple robot description using URDF

1.1 Introduction to ROS 2

What is ROS 2?

Robot Operating System 2 (ROS 2) is not an operating system in the traditional sense—it's a middleware framework that provides:

• Communication infrastructure between processes
• Standard message definitions for common robot data
• Tools for debugging, visualization, and logging
• Libraries for common robotics algorithms

Why ROS 2 (vs. ROS 1)?

ROS 1 was revolutionary but had limitations for production systems:

Feature	ROS 1	ROS 2
Real-time	No	Yes (with RT-capable OS)
Middleware	Custom (TCPROS)	DDS (industry standard)
Security	None	Built-in encryption/authentication
Multi-robot	Difficult	Native support
Embedded	Limited	First-class (micro-ROS)
Python	2.7 → 3.x (late)	Python 3 from day 1

For humanoid robots, real-time performance is critical. A robot with 30+ joints must control them synchronously at high frequency (>100 Hz). ROS 2 makes this possible.

ROS 2 Architecture

┌─────────────────────────────────────────┐
│           Your Application              │
│  ┌─────────────┐      ┌──────────────┐ │
│  │  Node A     │      │   Node B     │ │
│  │  (Python)   │      │   (C++)      │ │
│  └──────┬──────┘      └──────┬───────┘ │
│         │                    │         │
└─────────┼────────────────────┼─────────┘
          │                    │
┌─────────▼────────────────────▼─────────┐
│         ROS 2 Client Library           │
│         (rclpy, rclcpp)                │
└──────────────────┬─────────────────────┘
                   │
┌──────────────────▼─────────────────────┐
│      Data Distribution Service (DDS)   │
│      (Cyclone DDS / Fast DDS)         │
└────────────────────────────────────────┘

1.2 Core Concepts

Nodes

A node is a single process that performs a specific function. For a humanoid robot, you might have:

• joint_controller - Controls motor positions
• camera_driver - Publishes camera images
• visual_detector - Detects objects in images
• planner - Decides where to move
• logger - Records data for debugging

Design Principle: One node, one responsibility. This makes debugging easier and enables distributed computing.

Topics (Publish-Subscribe)

Topics enable one-to-many asynchronous communication.

Publisher (Camera Driver)  →  /camera/image_raw  →  Subscribers (Vision Nodes)

• Typed: Each topic has a message type (e.g., sensor_msgs/Image)
• Asynchronous: Publishers don't wait for subscribers
• Many-to-many: Multiple publishers and subscribers allowed

When to use: Sensor data, continuous state updates.

Services (Request-Response)

Services enable synchronous request-response communication.

Client  →  /reset_robot  →  Server
       ←  Response (success)

• Blocking: Client waits for response
• One-to-one: Only one server per service

When to use: Configuration, one-off commands, state queries.

Actions (Long-running Tasks)

Actions are like services but for tasks that take time and need feedback.

Client  →  /navigate_to_goal  →  Server
       ←  Feedback (distance remaining)
       ←  Result (success/failure)

• Cancelable: Client can abort mid-execution
• Feedback: Progress updates during execution
• Goal-based: Specify desired outcome, not how to achieve it

When to use: Navigation, manipulation, any task >1 second.

1.3 Hands-On: Creating Your First Node

Setting Up the Environment

First, ensure ROS 2 is installed and sourced:

# Source ROS 2 setup
source /opt/ros/humble/setup.bash

# Create a workspace
mkdir -p ~/humanoid_ws/src
cd ~/humanoid_ws

Creating a Python Package

cd ~/humanoid_ws/src
ros2 pkg create --build-type ament_python humanoid_control \
  --dependencies rclpy sensor_msgs

This creates a package with:

• package.xml - Dependencies and metadata
• setup.py - Python package configuration
• humanoid_control/ - Your Python modules

Writing a Joint State Publisher

Joint states represent the position/velocity/effort of each robot joint. For a humanoid with arms and legs, you might have 30+ joints.

humanoid_control/joint_publisher.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from math import sin, cos, pi
import time

class JointPublisher(Node):
    """
    Publishes joint states for a humanoid robot.
    Simulates a simple motion pattern for demonstration.
    """

    def __init__(self):
        super().__init__('joint_publisher')

        # Create publisher
        self.publisher_ = self.create_publisher(
            JointState,
            'joint_states',
            10  # Queue size
        )

        # Declare parameters
        self.declare_parameter('update_rate', 50.0)  # Hz

        # Get parameter value
        update_rate = self.get_parameter('update_rate').value
        timer_period = 1.0 / update_rate

        # Create timer
        self.timer = self.create_timer(timer_period, self.timer_callback)

        # State variables
        self.t = 0.0

        self.get_logger().info(
            f'Joint publisher started at {update_rate} Hz'
        )

    def timer_callback(self):
        """
        Called periodically to publish joint states.
        Implements a simple walking pattern.
        """
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'base_link'

        # Define joint names (simplified humanoid)
        msg.name = [
            # Torso
            'torso_yaw',
            # Right leg
            'right_hip_pitch', 'right_hip_roll', 'right_knee',
            'right_ankle_pitch', 'right_ankle_roll',
            # Left leg
            'left_hip_pitch', 'left_hip_roll', 'left_knee',
            'left_ankle_pitch', 'left_ankle_roll',
            # Right arm
            'right_shoulder_pitch', 'right_elbow',
            # Left arm
            'left_shoulder_pitch', 'left_elbow',
        ]

        # Simulate walking motion (simple sine waves)
        # Right leg leads, left leg follows (phase shift)
        right_phase = sin(self.t)
        left_phase = sin(self.t + pi)

        msg.position = [
            # Torso (slight rotation for balance)
            0.1 * sin(2 * self.t),

            # Right leg (lift when positive)
            0.3 * right_phase,  # hip pitch
            0.1 * right_phase,  # hip roll
            0.5 * max(0, right_phase),  # knee (only bend forward)
            -0.3 * right_phase,  # ankle pitch
            0.0,  # ankle roll

            # Left leg
            0.3 * left_phase,
            0.1 * left_phase,
            0.5 * max(0, left_phase),
            -0.3 * left_phase,
            0.0,

            # Arms (swing opposite to legs)
            -0.2 * right_phase,
            0.1,
            -0.2 * left_phase,
            0.1,
        ]

        # For a real robot, we'd also send velocity and effort
        msg.velocity = []  # Leave empty for simulation
        msg.effort = []

        self.publisher_.publish(msg)

        # Increment time
        self.t += self.timer_period
        if self.t > 2 * pi:
            self.t -= 2 * pi

def main(args=None):
    rclpy.init(args=args)
    node = JointPublisher()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Configuring the Package

Update setup.py to install the node:

setup.py

from setuptools import setup

package_name = 'humanoid_control'

setup(
    name=package_name,
    version='0.0.1',
    packages=[package_name],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Your Name',
    maintainer_email='your.email@example.com',
    description='Humanoid robot control nodes',
    license='Apache-2.0',
    entry_points={
        'console_scripts': [
            'joint_publisher = humanoid_control.joint_publisher:main',
        ],
    },
)

Building and Running

# Build the workspace
cd ~/humanoid_ws
colcon build --packages-select humanoid_control

# Source the workspace
source install/setup.bash

# Run the node
ros2 run humanoid_control joint_publisher

You should see:

[INFO] [joint_publisher]: Joint publisher started at 50.0 Hz

Inspecting Running System

In another terminal:

# List all nodes
ros2 node list

# List all topics
ros2 topic list

# See messages on joint_states topic
ros2 topic echo /joint_states

# See topic info (publishers, subscribers, message type)
ros2 topic info /joint_states

# Monitor publish rate
ros2 topic hz /joint_states

1.4 Visualizing with RViz2

RViz2 is the standard visualization tool for ROS 2.

# Install if not already installed
sudo apt install ros-humble-rviz2

# Launch
rviz2

1. Set Fixed Frame to base_link
2. Click Add → RobotModel
3. For the robot model to appear, you'll need a URDF (next section)

1.5 Introduction to URDF

Unified Robot Description Format (URDF) defines your robot's:

• Links: Rigid bodies (torso, thigh, shin, etc.)
• Joints: Connections between links (hip, knee, ankle)
• Visual: How it looks (meshes, colors)
• Collision: Simplified geometry for physics
• Inertia: Mass properties for dynamics

Minimal URDF Example

urdf/simple_humanoid.urdf

<?xml version="1.0"?>
<robot name="simple_humanoid">

  <!-- Base link (torso) -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.3 0.2 0.5"/>
      </geometry>
      <material name="blue">
        <color rgba="0.2 0.4 0.8 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.3 0.2 0.5"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="5.0"/>
      <inertia ixx="0.1" ixy="0.0" ixz="0.0"
               iyy="0.1" iyz="0.0" izz="0.1"/>
    </inertial>
  </link>

  <!-- Head -->
  <link name="head">
    <visual>
      <geometry>
        <sphere radius="0.12"/>
      </geometry>
      <material name="skin">
        <color rgba="0.9 0.8 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="0.12"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0"
               iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <!-- Neck joint -->
  <joint name="neck" type="revolute">
    <parent link="base_link"/>
    <child link="head"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1.0"/>
  </joint>

  <!-- Right thigh -->
  <link name="right_thigh">
    <visual>
      <geometry>
        <cylinder length="0.4" radius="0.05"/>
      </geometry>
      <origin xyz="0 0 -0.2" rpy="0 0 0"/>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
  </link>

  <!-- Right hip joint -->
  <joint name="right_hip" type="revolute">
    <parent link="base_link"/>
    <child link="right_thigh"/>
    <origin xyz="0.1 -0.15 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.57" upper="1.57" effort="100" velocity="2.0"/>
  </joint>

  <!-- Add more links/joints for complete humanoid... -->

</robot>

Publishing Robot State

To visualize the URDF in RViz, we need to publish:

1. Robot Description (the URDF itself)
2. TF transforms (joint angles → link positions)

humanoid_control/robot_state_publisher.py

import rclpy
from rclpy.node import Node

def main(args=None):
    rclpy.init(args=args)

    # For now, use the built-in robot_state_publisher
    # We'll create custom versions later

    print("Use: ros2 launch robot_state_publisher robot_state_publisher.launch.py")

if __name__ == '__main__':
    main()

Better approach - use launch files (Chapter 2).

Summary

In this chapter, you learned:

✅ The architecture and motivation behind ROS 2
✅ Core communication patterns (topics, services, actions)
✅ How to create a ROS 2 Python node
✅ Publishing joint states for a humanoid robot
✅ Basic URDF for robot description
✅ Visualization with RViz2

Key Takeaways

• Nodes are single-responsibility processes
• Topics are for continuous data (sensors, state)
• Services are for one-off requests
• Actions are for goal-based, long-running tasks
• URDF describes robot kinematics and appearance

What's Next?

In Chapter 2, we'll dive deeper into:

• Launch files for starting multiple nodes
• Parameters for configuration
• Lifecycle nodes for safety-critical systems
• Integrating sensors (cameras, LiDAR, IMU)

---

Exercises

Exercise 1: Modify the Pattern

Change the joint_publisher.py to make the robot:

• Wave with both arms
• Stand on one leg
• Do a jumping motion

Exercise 2: Add More Joints

Expand the URDF to include:

• Both complete legs (thigh, shin, foot)
• Both complete arms (upper arm, forearm, hand)

Exercise 3: Create a Subscriber

Write a node that subscribes to joint_states and prints the position of the right knee joint.

Hint: You'll need to filter msg.name to find the index, then access msg.position[index].

---

Further Reading

• ROS 2 Design Decisions
• DDS: The Middleware
• URDF Specification
• sensor_msgs API

Next: Chapter 2: Advanced ROS 2