Skip to main content

Chapter 3: Python-to-ROS Bridging via rclpy

Introduction​

Python is one of the most popular languages in robotics and AI, thanks to its simplicity, extensive libraries, and rapid prototyping capabilities. rclpy (ROS Client Library for Python) is the official Python binding for ROS 2, enabling you to leverage Python's power while accessing the full ROS 2 ecosystem.

ℹ️What You'll Learn
  • How rclpy bridges Python and ROS 2
  • Setting up a complete Python ROS 2 workspace
  • Creating publishers, subscribers, services, and timers
  • Working with parameters and lifecycle nodes
  • Building a complete robotic application in Python
  • Best practices for Python robotics development

Why Python for Robotics?​

Python offers unique advantages for robotics development:

Strengths​

  • Rapid Prototyping: Write and test code quickly
  • Rich Ecosystem: NumPy, SciPy, OpenCV, TensorFlow, PyTorch
  • Readability: Clean, maintainable code
  • Integration: Easy to interface with sensors, actuators, and APIs
  • Learning Curve: Gentle introduction for beginners

Considerations​

  • Performance: Slower than C++ for compute-intensive tasks
  • Type Safety: Dynamic typing can lead to runtime errors
  • Concurrency: GIL (Global Interpreter Lock) limits true parallelism
✅Best of Both Worlds

Use Python for high-level logic and AI/ML, and C++ for low-level, performance-critical components. ROS 2 makes this seamless!

Setting Up Your Python ROS 2 Workspace​

Directory Structure​

my_robot_workspace/
├── src/
│   └── my_robot_package/
│       ├── my_robot_package/
│       │   ├── __init__.py
│       │   ├── publisher_node.py
│       │   └── subscriber_node.py
│       ├── package.xml
│       ├── setup.py
│       └── setup.cfg
└── install/

Creating a Package​

# Navigate to workspace
cd ~/my_robot_workspace/src

# Create a Python package
ros2 pkg create my_robot_package --build-type ament_python --dependencies rclpy std_msgs

# Build the workspace
cd ~/my_robot_workspace
colcon build

# Source the workspace
source install/setup.bash

Package Configuration (setup.py)​

from setuptools import setup

package_name = 'my_robot_package'

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='My robot package',
    license='Apache License 2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'publisher_node = my_robot_package.publisher_node:main',
            'subscriber_node = my_robot_package.subscriber_node:main',
        ],
    },
)

The rclpy Node Lifecycle​

Understanding the node lifecycle is crucial for proper resource management:

import rclpy
from rclpy.node import Node

def main(args=None):
    # 1. Initialize the ROS 2 Python client library
    rclpy.init(args=args)

    # 2. Create node instance
    node = MyNode()

    try:
        # 3. Spin the node (process callbacks)
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # 4. Clean up
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Key Functions​

  • rclpy.init(): Initialize the ROS 2 Python context
  • Node(): Create a node instance
  • rclpy.spin(): Block and process callbacks
  • node.destroy_node(): Clean up node resources
  • rclpy.shutdown(): Shutdown the ROS 2 Python context
⚠️Always Clean Up

Forgetting to call destroy_node() and shutdown() can lead to resource leaks. Use try-finally blocks!

Building a Complete Example: Robot Velocity Controller​

Let's build a practical example that demonstrates multiple rclpy features.

1. Publisher: Velocity Command Node​

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import math

class VelocityPublisher(Node):
    def __init__(self):
        super().__init__('velocity_publisher')

        # Create publisher
        self.publisher = self.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )

        # Create timer (10 Hz)
        self.timer = self.create_timer(0.1, self.publish_velocity)

        # State variables
        self.time = 0.0

        self.get_logger().info('Velocity Publisher started')

    def publish_velocity(self):
        msg = Twist()

        # Create circular motion
        msg.linear.x = 0.5  # Forward velocity
        msg.angular.z = math.sin(self.time) * 0.5  # Sinusoidal turn

        self.publisher.publish(msg)

        self.time += 0.1

        if self.time >= 2 * math.pi:
            self.time = 0.0

def main(args=None):
    rclpy.init(args=args)
    node = VelocityPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

2. Subscriber: Velocity Monitor Node​

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import math

class VelocityMonitor(Node):
    def __init__(self):
        super().__init__('velocity_monitor')

        # Create subscriber
        self.subscription = self.create_subscription(
            Twist,
            '/cmd_vel',
            self.velocity_callback,
            10
        )

        # Declare parameters
        self.declare_parameter('max_linear_speed', 1.0)
        self.declare_parameter('max_angular_speed', 2.0)

        self.get_logger().info('Velocity Monitor started')

    def velocity_callback(self, msg):
        # Get parameters
        max_linear = self.get_parameter('max_linear_speed').value
        max_angular = self.get_parameter('max_angular_speed').value

        # Check limits
        if abs(msg.linear.x) > max_linear:
            self.get_logger().warn(
                f'Linear velocity {msg.linear.x:.2f} exceeds max {max_linear:.2f}'
            )

        if abs(msg.angular.z) > max_angular:
            self.get_logger().warn(
                f'Angular velocity {msg.angular.z:.2f} exceeds max {max_angular:.2f}'
            )

        # Calculate speed
        speed = math.sqrt(msg.linear.x**2 + msg.linear.y**2)

        self.get_logger().info(
            f'Speed: {speed:.2f} m/s, Turn: {msg.angular.z:.2f} rad/s'
        )

def main(args=None):
    rclpy.init(args=args)
    node = VelocityMonitor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

3. Service: Emergency Stop​

import rclpy
from rclpy.node import Node
from std_srvs.srv import Trigger

class EmergencyStopService(Node):
    def __init__(self):
        super().__init__('emergency_stop_service')

        # Create service
        self.service = self.create_service(
            Trigger,
            'emergency_stop',
            self.emergency_stop_callback
        )

        self.is_stopped = False

        self.get_logger().info('Emergency Stop Service ready')

    def emergency_stop_callback(self, request, response):
        self.is_stopped = True

        response.success = True
        response.message = 'Emergency stop activated!'

        self.get_logger().error('EMERGENCY STOP ACTIVATED!')

        return response

def main(args=None):
    rclpy.init(args=args)
    node = EmergencyStopService()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Advanced rclpy Features​

1. Multi-Threaded Executors​

For concurrent callback execution:

from rclpy.executors import MultiThreadedExecutor

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

    node1 = Node1()
    node2 = Node2()

    executor = MultiThreadedExecutor()
    executor.add_node(node1)
    executor.add_node(node2)

    try:
        executor.spin()
    finally:
        executor.shutdown()
        node1.destroy_node()
        node2.destroy_node()
        rclpy.shutdown()

2. Parameter Callbacks​

React to parameter changes:

from rcl_interfaces.msg import ParameterDescriptor

class ParameterNode(Node):
    def __init__(self):
        super().__init__('parameter_node')

        # Declare parameter with description
        descriptor = ParameterDescriptor(
            description='Maximum speed in m/s'
        )
        self.declare_parameter('max_speed', 1.0, descriptor)

        # Add parameter callback
        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, params):
        for param in params:
            if param.name == 'max_speed':
                if param.value < 0:
                    self.get_logger().error('Speed cannot be negative!')
                    return SetParametersResult(successful=False)
                self.get_logger().info(f'Max speed updated to {param.value}')

        return SetParametersResult(successful=True)

3. Action Servers​

For long-running tasks with feedback:

import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from example_interfaces.action import Fibonacci

class FibonacciActionServer(Node):
    def __init__(self):
        super().__init__('fibonacci_action_server')

        self._action_server = ActionServer(
            self,
            Fibonacci,
            'fibonacci',
            self.execute_callback
        )

    def execute_callback(self, goal_handle):
        self.get_logger().info('Executing goal...')

        feedback_msg = Fibonacci.Feedback()
        feedback_msg.sequence = [0, 1]

        for i in range(1, goal_handle.request.order):
            feedback_msg.sequence.append(
                feedback_msg.sequence[i] + feedback_msg.sequence[i-1]
            )

            self.get_logger().info(f'Feedback: {feedback_msg.sequence}')
            goal_handle.publish_feedback(feedback_msg)

        goal_handle.succeed()

        result = Fibonacci.Result()
        result.sequence = feedback_msg.sequence
        return result
ℹ️Actions

Actions are like services but support feedback and cancellation. Perfect for long-running tasks like navigation!

Integration with Python Libraries​

Computer Vision with OpenCV​

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImageProcessor(Node):
    def __init__(self):
        super().__init__('image_processor')

        self.bridge = CvBridge()

        self.subscription = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.image_callback,
            10
        )

    def image_callback(self, msg):
        # Convert ROS Image to OpenCV
        cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

        # Process image
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        edges = cv2.Canny(gray, 50, 150)

        # Display (for debugging)
        cv2.imshow('Edges', edges)
        cv2.waitKey(1)

Machine Learning with NumPy/TensorFlow​

import numpy as np
from tensorflow import keras

class MLNode(Node):
    def __init__(self):
        super().__init__('ml_node')

        # Load pre-trained model
        self.model = keras.models.load_model('my_model.h5')

        self.subscription = self.create_subscription(
            sensor_msgs.msg.LaserScan,
            '/scan',
            self.scan_callback,
            10
        )

    def scan_callback(self, msg):
        # Preprocess sensor data
        data = np.array(msg.ranges)
        data = data.reshape(1, -1)

        # Predict
        prediction = self.model.predict(data)

        self.get_logger().info(f'Prediction: {prediction}')

Testing rclpy Nodes​

Unit Testing with pytest​

import pytest
import rclpy
from my_robot_package.publisher_node import VelocityPublisher

@pytest.fixture
def node():
    rclpy.init()
    node = VelocityPublisher()
    yield node
    node.destroy_node()
    rclpy.shutdown()

def test_publisher_creation(node):
    assert node.publisher is not None

def test_timer_frequency(node):
    assert node.timer.timer_period_ns == 100000000  # 10 Hz

Best Practices​

1. Use Type Hints​

from typing import Optional
from geometry_msgs.msg import Twist

def process_velocity(self, msg: Twist) -> Optional[float]:
    return msg.linear.x if msg else None

2. Proper Logging​

self.get_logger().debug('Detailed debug info')
self.get_logger().info('Important event')
self.get_logger().warn('Warning message')
self.get_logger().error('Error occurred')
self.get_logger().fatal('Critical failure')

3. Resource Management​

class ResourceNode(Node):
    def __init__(self):
        super().__init__('resource_node')
        self.file = open('data.txt', 'w')

    def __del__(self):
        if hasattr(self, 'file'):
            self.file.close()

4. Error Handling​

def safe_callback(self, msg):
    try:
        self.process_message(msg)
    except Exception as e:
        self.get_logger().error(f'Error processing message: {e}')

Key Takeaways​

  • rclpy bridges Python and ROS 2, enabling rapid robotics development
  • Python packages follow a specific structure with setup.py and package.xml
  • Always properly initialize and shutdown rclpy
  • Use executors for concurrent callback processing
  • Integrate seamlessly with Python libraries (NumPy, OpenCV, TensorFlow)
  • Follow best practices: type hints, logging, error handling

What's Next?​

Now that you've mastered Python development with rclpy, in Chapter 4 we'll explore URDF for Humanoids – learning how to define and visualize robot structures using the Unified Robot Description Format!

Additional Resources​

Exercises​

  1. Create a turtle bot controller that moves in a square pattern
  2. Implement a parameter server that stores robot configuration
  3. Build an action server for a "go to position" task
  4. Integrate OpenCV to detect colored objects from camera feed
  5. Write unit tests for your nodes using pytest