rclpy Integration

Why Python for Robotics?

While C++ (rclcpp) offers maximum performance, Python (rclpy) excels at:

• Rapid Prototyping: Test ideas in minutes, not hours
• ML Integration: Seamless PyTorch, TensorFlow, NumPy, OpenCV integration
• Readability: Easier debugging and collaboration
• Ecosystem: Access to 400,000+ PyPI packages

Trade-offs: Python is 10-100x slower than C++ for compute-intensive tasks. Use Python for high-level logic; offload heavy lifting to C++ nodes or GPU-accelerated libraries.

---

Node Lifecycle

Basic Node Structure

Every rclpy node follows this pattern:

template_node.py

import rclpy
from rclpy.node import Node

class TemplateNode(Node):
    def __init__(self):
        # 1. Initialize base class with node name
        super().__init__('template_node')

        # 2. Declare parameters
        self.declare_parameter('update_rate', 10.0)

        # 3. Create publishers/subscribers/services
        self.setup_communication()

        # 4. Initialize state variables
        self.counter = 0

        # 5. Create timers for periodic tasks
        rate = self.get_parameter('update_rate').value
        self.timer = self.create_timer(1.0 / rate, self.timer_callback)

        self.get_logger().info('Node initialized')

    def setup_communication(self):
        # Publishers, subscribers, etc.
        pass

    def timer_callback(self):
        self.counter += 1
        self.get_logger().info(f'Counter: {self.counter}')

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

    # Create node instance
    node = TemplateNode()

    try:
        # Spin (process callbacks)
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        # Cleanup
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

Key Points:

• super().__init__('node_name'): Register node with ROS 2
• rclpy.spin(node): Enter event loop, processing callbacks
• destroy_node(): Release resources (publishers, timers)
• rclpy.shutdown(): Cleanup rclpy context

---

Timers: Periodic Execution

Timers trigger callbacks at fixed intervals—essential for sensor polling, control loops, and heartbeat signals.

periodic_publisher.py

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from datetime import datetime

class PeriodicPublisher(Node):
    def __init__(self):
        super().__init__('periodic_publisher')

        # Publish at 5 Hz (every 200ms)
        self.timer_period = 0.2
        self.publisher_ = self.create_publisher(String, 'status', 10)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        self.loop_count = 0

    def timer_callback(self):
        msg = String()
        msg.data = f'Heartbeat {self.loop_count} at {datetime.now()}'

        self.publisher_.publish(msg)
        self.loop_count += 1

def main(args=None):
    rclpy.init(args=args)
    node = PeriodicPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

Timer Precision: Python timers have ~1ms jitter. For sub-millisecond precision, use C++ nodes or real-time Linux kernels.

---

Executors: Advanced Callback Management

Single-Threaded Executor (Default)

# Default behavior
rclpy.spin(node)  # Processes callbacks sequentially

Problem: If callback_A() takes 500ms, callback_B() is delayed.

Multi-Threaded Executor

from rclpy.executors import MultiThreadedExecutor

executor = MultiThreadedExecutor()
executor.add_node(node)

try:
    executor.spin()
finally:
    executor.shutdown()
    node.destroy_node()
    rclpy.shutdown()

When to Use:

• Multiple slow callbacks (image processing, ML inference)
• Callbacks that can run in parallel

Caution: Thread safety! Use threading.Lock() for shared state.

---

Logging: Debugging and Monitoring

rclpy provides five log levels:

Level	Method	Use Case
DEBUG	get_logger().debug()	Verbose internal state (disabled in release builds)
INFO	get_logger().info()	Normal operational messages
WARN	get_logger().warn()	Recoverable errors (e.g., sensor timeout)
ERROR	get_logger().error()	Unrecoverable errors (node continues)
FATAL	get_logger().fatal()	Critical failures (node should shutdown)

logging_example.py

import rclpy
from rclpy.node import Node

class LoggingNode(Node):
    def __init__(self):
        super().__init__('logging_demo')

        # Set log level (default: INFO)
        self.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG)

        # Different log levels
        self.get_logger().debug('Debug: Variable x = 42')
        self.get_logger().info('System initialized successfully')
        self.get_logger().warn('Battery level low: 15%')
        self.get_logger().error('Failed to connect to sensor')
        self.get_logger().fatal('Emergency stop activated!')

        # Formatted logging
        battery_pct = 15
        self.get_logger().warn(f'Battery at {battery_pct}%')

        # Throttled logging (max 1 msg per second)
        self.timer = self.create_timer(0.1, self.throttled_log)

    def throttled_log(self):
        # Only logs once per second despite 10 Hz timer
        self.get_logger().info(
            'High-frequency message',
            throttle_duration_sec=1.0
        )

def main(args=None):
    rclpy.init(args=args)
    node = LoggingNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

Viewing Logs:

# Console output (colored, formatted)
ros2 run my_package logging_demo

# rqt_console (GUI log viewer)
rqt_console

# Filter by severity
ros2 run my_package my_node --ros-args --log-level DEBUG

---

Parameter Management

Declaring Parameters

parameter_node.py

import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import ParameterDescriptor

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

        # Declare with default value
        self.declare_parameter('robot_name', 'Atlas')

        # Declare with description (shows in param list)
        descriptor = ParameterDescriptor(
            description='Maximum velocity in m/s',
            additional_constraints='Must be positive'
        )
        self.declare_parameter('max_velocity', 2.0, descriptor)

        # Declare with validation
        self.declare_parameter('lidar_enabled', True)

        # Read parameters
        self.robot_name = self.get_parameter('robot_name').value
        self.max_vel = self.get_parameter('max_velocity').value

        self.get_logger().info(f'Robot: {self.robot_name}, Max vel: {self.max_vel}')

        # Parameter change callback
        self.add_on_set_parameters_callback(self.parameter_callback)

    def parameter_callback(self, params):
        for param in params:
            if param.name == 'max_velocity' and param.value < 0:
                self.get_logger().error('max_velocity must be positive!')
                return rclpy.parameter.SetParametersResult(successful=False)

        return rclpy.parameter.SetParametersResult(successful=True)

def main(args=None):
    rclpy.init(args=args)
    node = ParameterNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

Setting Parameters at Launch

# Command line
ros2 run my_package parameter_node --ros-args -p robot_name:=Optimus -p max_velocity:=3.5

# From YAML file (params.yaml)
# /parameter_node:
#   ros__parameters:
#     robot_name: "Digit"
#     max_velocity: 1.5

ros2 run my_package parameter_node --ros-args --params-file params.yaml

Runtime Parameter Changes

# Get current value
ros2 param get /parameter_node robot_name

# Set new value
ros2 param set /parameter_node robot_name "Figure01"

# Dump all parameters
ros2 param dump /parameter_node

---

Context Managers: Cleaner Code

context_manager_example.py

import rclpy
from rclpy.node import Node

class ManagedNode(Node):
    def __init__(self):
        super().__init__('managed_node')
        self.get_logger().info('Node started')

    def __enter__(self):
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.get_logger().info('Shutting down gracefully')
        self.destroy_node()

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

    with ManagedNode() as node:
        rclpy.spin(node)

    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

Practical Example: Sensor Fusion Node

sensor_fusion.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import Twist
from rclpy.qos import QoSProfile, ReliabilityPolicy
import numpy as np

class SensorFusionNode(Node):
    def __init__(self):
        super().__init__('sensor_fusion')

        # QoS for sensor data (best effort)
        sensor_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT
        )

        # Subscribers
        self.lidar_sub = self.create_subscription(
            LaserScan,
            '/scan',
            self.lidar_callback,
            sensor_qos
        )

        self.imu_sub = self.create_subscription(
            Imu,
            '/imu',
            self.imu_callback,
            sensor_qos
        )

        # Publisher for velocity commands
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # State
        self.min_obstacle_dist = float('inf')
        self.angular_velocity = 0.0

        # Fusion timer (20 Hz)
        self.timer = self.create_timer(0.05, self.fusion_callback)

        self.get_logger().info('Sensor fusion node ready')

    def lidar_callback(self, msg):
        # Extract minimum distance
        valid_ranges = [r for r in msg.ranges if msg.range_min < r < msg.range_max]
        if valid_ranges:
            self.min_obstacle_dist = min(valid_ranges)

    def imu_callback(self, msg):
        # Extract angular velocity (z-axis)
        self.angular_velocity = msg.angular_velocity.z

    def fusion_callback(self):
        # Combine LiDAR + IMU for obstacle avoidance
        cmd = Twist()

        if self.min_obstacle_dist < 0.5:  # Obstacle within 50cm
            # Stop forward motion
            cmd.linear.x = 0.0

            # Rotate to avoid
            cmd.angular.z = 0.5 if self.angular_velocity > 0 else -0.5

            self.get_logger().warn(
                f'Obstacle at {self.min_obstacle_dist:.2f}m - avoiding'
            )
        else:
            # Move forward
            cmd.linear.x = 0.3
            cmd.angular.z = 0.0

        self.cmd_pub.publish(cmd)

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

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

if __name__ == '__main__':
    main()

---

Best Practices

1. Use Type Hints

from typing import Optional
from sensor_msgs.msg import LaserScan

def process_scan(self, msg: LaserScan) -> Optional[float]:
    if not msg.ranges:
        return None
    return min(msg.ranges)

2. Handle Shutdown Gracefully

import signal

def signal_handler(sig, frame):
    rclpy.shutdown()

signal.signal(signal.SIGINT, signal_handler)

3. Test Nodes Individually

# tests/test_sensor_fusion.py
import pytest
import rclpy
from my_package.sensor_fusion import SensorFusionNode

def test_obstacle_detection():
    rclpy.init()
    node = SensorFusionNode()

    # Inject test data
    node.min_obstacle_dist = 0.3  # Simulate close obstacle

    # Verify behavior
    assert node.min_obstacle_dist < 0.5

    node.destroy_node()
    rclpy.shutdown()

---

Debugging Tips

1. Enable Debug Logging

ros2 run my_package my_node --ros-args --log-level DEBUG

2. Inspect Node Graph

# List nodes
ros2 node list

# Node details
ros2 node info /sensor_fusion

# Topic connections
rqt_graph

3. Profile Performance

import time

def timer_callback(self):
    start = time.perf_counter()

    # Your logic here

    duration = (time.perf_counter() - start) * 1000  # ms
    if duration > 50:  # 50ms threshold
        self.get_logger().warn(f'Slow callback: {duration:.2f}ms')

---

Key Takeaways

✅ Node Structure: Init → Setup → Spin → Cleanup

✅ Timers: Periodic execution with configurable rates

✅ Executors: Single-threaded (default) vs. multi-threaded (parallel callbacks)

✅ Logging: Use appropriate severity levels, throttle high-frequency logs

✅ Parameters: Declare with defaults, validate changes, support runtime tuning

---

Next Steps

Now that you can build Python nodes, let's model physical robots using URDF.

Continue to URDF for Humanoids →