Lab Exercise: ROS 2 Node Development for Humanoid Control

Objective

In this lab, you'll create a simple ROS 2 node to demonstrate fundamental concepts of ROS 2 architecture and communication patterns relevant to humanoid robotics.

Prerequisites

• ROS 2 Humble Hawksbill or Iron Irwini installed
• Basic Python or C++ programming skills
• Understanding of ROS 2 concepts from the chapter

Theory

ROS 2 provides the communication infrastructure for humanoid robots using nodes, topics, services, and actions. This lab demonstrates how to create a node that simulates a humanoid controller.

Implementation

Step 1: Create a ROS 2 Package

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python humanoid_control_pkg
cd humanoid_control_pkg

Step 2: Create the Controller Node

Create the main controller node that will simulate humanoid joint control:

# humanoid_control_pkg/humanoid_control_pkg/humanoid_controller.py
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from sensor_msgs.msg import JointState
from builtin_interfaces.msg import Duration
from rclpy.qos import qos_profile_system_default
import time
import math

class HumanoidController(Node):
    def __init__(self):
        super().__init__('humanoid_controller')
        
        # Create publishers for joint commands
        self.joint_cmd_publisher = self.create_publisher(
            Float64MultiArray,
            '/joint_commands',
            qos_profile=qos_profile_system_default
        )
        
        # Create subscriber for joint states
        self.joint_state_subscriber = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            qos_profile=qos_profile_system_default
        )
        
        # Timer to periodically send commands
        self.timer = self.create_timer(0.1, self.send_joint_commands)
        
        # Initialize joint positions
        self.joint_positions = [0.0] * 32  # 32 DoF for humanoid
        self.step_counter = 0
        
        self.get_logger().info('Humanoid Controller initialized')

    def joint_state_callback(self, msg):
        """Callback to receive joint state updates"""
        self.joint_positions = list(msg.position)
        self.get_logger().info(f'Received joint states: {len(msg.position)} joints')

    def send_joint_commands(self):
        """Send joint commands to simulate humanoid movement"""
        # Create a simple walking gait pattern
        msg = Float64MultiArray()
        
        # Simulate a basic walking pattern (simplified for demonstration)
        for i in range(32):
            # Create different movement patterns for different joints
            if i < 4:  # Hip joints
                msg.data.append(math.sin(self.step_counter * 0.1 + i) * 0.2)
            elif i < 8:  # Knee joints
                msg.data.append(math.sin(self.step_counter * 0.1 + i + 0.5) * 0.15)
            elif i < 12:  # Ankle joints
                msg.data.append(math.sin(self.step_counter * 0.1 + i + 1.0) * 0.1)
            elif i < 16:  # Hip sway
                msg.data.append(math.sin(self.step_counter * 0.1 + i + 1.5) * 0.05)
            else:  # Other joints (arms, head, etc.)
                msg.data.append(0.0)
        
        self.joint_cmd_publisher.publish(msg)
        self.step_counter += 1


def main(args=None):
    rclpy.init(args=args)
    controller = HumanoidController()
    
    try:
        rclpy.spin(controller)
    except KeyboardInterrupt:
        controller.get_logger().info('Shutting down Humanoid Controller')
    finally:
        controller.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Step 3: Create a Service Node

Create a service node that can respond to requests to move to specific poses:

# humanoid_control_pkg/humanoid_control_pkg/pose_service.py
import rclpy
from rclpy.node import Node
from example_interfaces.srv import Trigger
from std_msgs.msg import Float64MultiArray
import math

class PoseService(Node):
    def __init__(self):
        super().__init__('pose_service')
        
        # Create service
        self.srv = self.create_service(
            Trigger,
            'move_to_pose',
            self.move_to_pose_callback
        )
        
        # Publisher for joint commands
        self.joint_cmd_publisher = self.create_publisher(
            Float64MultiArray,
            '/joint_commands',
            10
        )
        
        self.get_logger().info('Pose Service initialized')

    def move_to_pose_callback(self, request, response):
        """Service callback to move humanoid to a specific pose"""
        self.get_logger().info('Received move_to_pose request')
        
        # Create a "wave" pose as an example
        msg = Float64MultiArray()
        for i in range(32):
            if i == 16:  # Right shoulder
                msg.data.append(math.radians(90))  # Raise right arm
            elif i == 17:  # Right elbow
                msg.data.append(math.radians(-90))  # Bend elbow
            else:
                msg.data.append(0.0)  # Default position
        
        # Publish the pose for 2 seconds
        for _ in range(20):  # Publish for 2 seconds at 10 Hz
            self.joint_cmd_publisher.publish(msg)
            time.sleep(0.1)
        
        response.success = True
        response.message = 'Moved to wave pose'
        return response


def main(args=None):
    rclpy.init(args=args)
    service = PoseService()
    
    try:
        rclpy.spin(service)
    except KeyboardInterrupt:
        service.get_logger().info('Shutting down Pose Service')
    finally:
        service.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Step 4: Update setup.py

# humanoid_control_pkg/setup.py
from setuptools import find_packages, setup

package_name = 'humanoid_control_pkg'

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']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='your_name',
    maintainer_email='your_email@example.com',
    description='Humanoid control package for ROS 2 lab',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'humanoid_controller = humanoid_control_pkg.humanoid_controller:main',
            'pose_service = humanoid_control_pkg.pose_service:main',
        ],
    },
)

Step 5: Build and Run

cd ~/ros2_ws
colcon build --packages-select humanoid_control_pkg
source install/setup.bash

# Run the controller
ros2 run humanoid_control_pkg humanoid_controller

# In another terminal, run the service
ros2 run humanoid_control_pkg pose_service

# In a third terminal, call the service
ros2 service call /move_to_pose example_interfaces/srv/Trigger

Results and Discussion

1. How does the publisher-subscriber model facilitate communication in your humanoid controller?
2. What are the advantages of using services for specific pose commands?
3. How could you extend this example to control a real humanoid robot?

Extensions

• Add action servers for complex, long-running behaviors
• Implement feedback control to maintain balance
• Add safety checks to prevent dangerous joint positions

Conclusion

This lab demonstrates fundamental ROS 2 concepts including nodes, publishers, subscribers, and services in the context of humanoid robot control. Understanding these communication patterns is essential for developing complex humanoid robot systems.