Embedded Testing

ROS2 Testing: How to Test Robot Software with pytest, Launch Tests, and Simulation

HelpMeTest

HelpMeTest

— 6 min read
ROS2 Testing: How to Test Robot Software with pytest, Launch Tests, and Simulation

Robot software is some of the hardest software to test. It's concurrent, time-sensitive, hardware-dependent, and the behavior emerges from complex interactions between perception, planning, and control. Yet untested robot software fails in the real world — sometimes expensively.

ROS2 (Robot Operating System 2) provides a testing framework that's underused by most teams. This guide covers practical ROS2 testing: unit tests for nodes, integration tests with launch testing, and simulation-based testing with Gazebo.

ROS2 Testing Architecture

ROS2 provides three testing levels:

  1. Unit tests — test individual nodes in isolation, mock topics and services
  2. Launch tests — spin up real ROS2 nodes, test their interactions
  3. Simulation tests — run the full stack in Gazebo simulation

Setting Up Testing

# Install testing dependencies
sudo apt install ros-humble-launch-testing
pip install pytest-ros pytest-asyncio

Unit Testing ROS2 Nodes

Unit test a node by spinning it in isolation with mocked I/O:

# tests/test_obstacle_detector.py
import pytest
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool
from my_robot.obstacle_detector import ObstacleDetectorNode
import numpy as np

@pytest.fixture(scope="session")
def ros_context():
    rclpy.init()
    yield
    rclpy.shutdown()

@pytest.fixture
def node(ros_context):
    node = ObstacleDetectorNode()
    yield node
    node.destroy_node()

def make_laser_scan(ranges, angle_min=-1.57, angle_max=1.57):
    """Helper to create a LaserScan message."""
    msg = LaserScan()
    msg.header.frame_id = "laser"
    msg.angle_min = angle_min
    msg.angle_max = angle_max
    msg.angle_increment = (angle_max - angle_min) / len(ranges)
    msg.range_min = 0.1
    msg.range_max = 10.0
    msg.ranges = [float(r) for r in ranges]
    return msg

def test_no_obstacle_when_clear(node):
    """Node should not detect obstacle when path is clear."""
    scan = make_laser_scan([5.0] * 100)  # All readings at 5m
    
    result = node.process_scan(scan)
    
    assert result.data == False, "Should not detect obstacle at 5m"

def test_obstacle_detected_close_range(node):
    """Node should detect obstacle within safety threshold."""
    scan = make_laser_scan([0.3] * 100)  # All readings at 0.3m
    
    result = node.process_scan(scan)
    
    assert result.data == True, "Should detect obstacle at 0.3m"

def test_single_close_reading_triggers_detection(node):
    """Even a single close reading in the frontal arc should trigger."""
    ranges = [5.0] * 100
    ranges[50] = 0.4  # One close reading in front
    
    scan = make_laser_scan(ranges)
    result = node.process_scan(scan)
    
    assert result.data == True, "Single close frontal reading should trigger obstacle"

def test_close_reading_behind_robot_ignored(node):
    """Readings behind the robot should not trigger frontal obstacle detection."""
    # Frontal arc: -0.5 to +0.5 rad
    # Reading at -1.5 rad = behind/side
    ranges = [5.0] * 100
    ranges[0] = 0.3  # Close reading at -1.57 rad (far left/behind)
    
    scan = make_laser_scan(ranges)
    result = node.process_scan(scan)
    
    assert result.data == False, "Rear reading should not trigger frontal obstacle"

def test_nan_ranges_handled_gracefully(node):
    """NaN ranges from sensor must not crash the node."""
    ranges = [float('nan')] * 100
    scan = make_laser_scan(ranges)
    
    # Should not raise an exception
    result = node.process_scan(scan)
    assert result is not None

Integration Testing with Launch Testing

Launch tests spin up real ROS2 nodes and test their pub/sub interactions:

# tests/test_navigation_integration.launch.py
import unittest
import pytest
import rclpy
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_testing.actions import ReadyToTest
import launch_testing

def generate_launch_description():
    """Launch the nodes under test."""
    obstacle_detector = Node(
        package="my_robot",
        executable="obstacle_detector",
        name="obstacle_detector",
        parameters=[{"safety_distance": 0.5}],
    )
    
    nav_controller = Node(
        package="my_robot",
        executable="navigation_controller",
        name="navigation_controller",
    )
    
    return LaunchDescription([
        obstacle_detector,
        nav_controller,
        ReadyToTest(),
    ])

class TestNavigationIntegration(unittest.TestCase):
    
    @classmethod
    def setUpClass(cls):
        rclpy.init()
        cls.node = rclpy.create_node("test_node")
    
    @classmethod
    def tearDownClass(cls):
        cls.node.destroy_node()
        rclpy.shutdown()
    
    def test_nodes_alive(self, proc_info):
        """Both nodes should be running."""
        self.assertIsNotNone(
            self.node.get_service_names_and_types(),
            "ROS2 graph should be reachable"
        )
    
    def test_obstacle_stops_navigation(self, proc_info):
        """Publishing obstacle detection should stop the robot."""
        import time
        from std_msgs.msg import Bool
        from geometry_msgs.msg import Twist
        
        velocity_commands = []
        
        # Subscribe to velocity commands
        sub = self.node.create_subscription(
            Twist, "/cmd_vel",
            lambda msg: velocity_commands.append(msg),
            10
        )
        
        # Publish obstacle detected
        pub = self.node.create_publisher(Bool, "/obstacle_detected", 10)
        time.sleep(0.5)
        
        pub.publish(Bool(data=True))
        time.sleep(1.0)
        
        rclpy.spin_once(self.node, timeout_sec=0.1)
        
        # Check that velocity commands went to zero
        if velocity_commands:
            last_cmd = velocity_commands[-1]
            assert last_cmd.linear.x == 0.0, "Linear velocity should stop on obstacle"
            assert last_cmd.angular.z == 0.0, "Angular velocity should stop on obstacle"

@pytest.mark.launch(generate_launch_description)
class TestNavigationIntegrationLaunch(TestNavigationIntegration):
    pass

Run launch tests:

ros2 launch my_robot_tests test_navigation_integration.launch.py
# or via pytest
pytest tests/test_navigation_integration.launch.py -v

Testing Services and Actions

# tests/test_robot_services.py
import pytest
import rclpy
from rclpy.action import ActionClient
from my_robot_interfaces.action import Navigate
from my_robot_interfaces.srv import GetPose

@pytest.fixture
def test_node():
    rclpy.init()
    node = rclpy.create_node("service_test_node")
    yield node
    node.destroy_node()
    rclpy.shutdown()

def test_get_pose_service_available(test_node):
    """GetPose service must be available."""
    client = test_node.create_client(GetPose, "/get_robot_pose")
    
    available = client.wait_for_service(timeout_sec=5.0)
    assert available, "GetPose service not available after 5 seconds"

def test_get_pose_returns_valid_response(test_node):
    """GetPose should return a valid pose."""
    import asyncio
    
    client = test_node.create_client(GetPose, "/get_robot_pose")
    client.wait_for_service(timeout_sec=5.0)
    
    request = GetPose.Request()
    future = client.call_async(request)
    
    rclpy.spin_until_future_complete(test_node, future, timeout_sec=5.0)
    
    assert future.done(), "Service call did not complete"
    response = future.result()
    
    # Pose values should be finite numbers
    assert -1e6 < response.pose.position.x < 1e6
    assert -1e6 < response.pose.position.y < 1e6

Simulation-Based Testing with Gazebo

For full integration tests without physical hardware:

# tests/test_gazebo_integration.py
import pytest
import subprocess
import time
import rclpy
from geometry_msgs.msg import PoseWithCovarianceStamped

@pytest.fixture(scope="session")
def gazebo_simulation():
    """Start Gazebo simulation and yield when ready."""
    # Launch Gazebo with test world
    proc = subprocess.Popen([
        "ros2", "launch", "my_robot_sim",
        "test_world.launch.py",
        "gui:=false",  # Headless for CI
    ])
    
    # Wait for simulation to be ready
    time.sleep(10)  # Gazebo startup time
    
    yield proc
    
    proc.terminate()
    proc.wait()

def test_robot_reaches_goal(gazebo_simulation):
    """Robot should navigate to a goal pose."""
    rclpy.init()
    nav_node = rclpy.create_node("navigation_test")
    
    goal_reached = []
    
    def pose_callback(msg):
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        # Check if within 0.3m of goal (3.0, 2.0)
        if abs(x - 3.0) < 0.3 and abs(y - 2.0) < 0.3:
            goal_reached.append(True)
    
    nav_node.create_subscription(
        PoseWithCovarianceStamped,
        "/amcl_pose",
        pose_callback,
        10
    )
    
    # Send navigation goal
    from my_robot_interfaces.action import Navigate
    from rclpy.action import ActionClient
    
    action_client = ActionClient(nav_node, Navigate, "/navigate_to_pose")
    action_client.wait_for_server(timeout_sec=10.0)
    
    goal = Navigate.Goal()
    goal.target_pose.pose.position.x = 3.0
    goal.target_pose.pose.position.y = 2.0
    
    future = action_client.send_goal_async(goal)
    
    # Wait for robot to reach goal (30 second timeout)
    deadline = time.time() + 30
    while not goal_reached and time.time() < deadline:
        rclpy.spin_once(nav_node, timeout_sec=0.5)
    
    nav_node.destroy_node()
    rclpy.shutdown()
    
    assert goal_reached, "Robot did not reach goal within 30 seconds"

Testing Message Timing and Rates

For real-time robot systems, message rates matter:

# tests/test_message_timing.py
import time
import rclpy
from sensor_msgs.msg import Imu

def test_imu_publishes_at_correct_rate(ros_node):
    """IMU should publish at 100Hz (±10%)."""
    timestamps = []
    
    def callback(msg):
        timestamps.append(time.time())
    
    sub = ros_node.create_subscription(Imu, "/imu/data", callback, 100)
    
    # Collect 2 seconds of data
    start = time.time()
    while time.time() - start < 2.0:
        rclpy.spin_once(ros_node, timeout_sec=0.01)
    
    assert len(timestamps) >= 10, f"Only received {len(timestamps)} IMU messages in 2s"
    
    # Compute actual rate
    duration = timestamps[-1] - timestamps[0]
    actual_rate = (len(timestamps) - 1) / duration
    
    expected_rate = 100.0
    assert abs(actual_rate - expected_rate) / expected_rate <= 0.10, \
        f"IMU rate {actual_rate:.1f} Hz, expected {expected_rate:.0f} ± 10%"

def test_camera_latency(ros_node):
    """Camera image should arrive within 100ms of capture."""
    from sensor_msgs.msg import Image
    latencies = []
    
    def callback(msg):
        now = ros_node.get_clock().now()
        msg_time = rclpy.time.Time.from_msg(msg.header.stamp)
        latency_ms = (now - msg_time).nanoseconds / 1e6
        latencies.append(latency_ms)
    
    sub = ros_node.create_subscription(Image, "/camera/image_raw", callback, 10)
    
    start = time.time()
    while time.time() - start < 3.0:
        rclpy.spin_once(ros_node, timeout_sec=0.01)
    
    if not latencies:
        pytest.skip("No camera images received")
    
    avg_latency = sum(latencies) / len(latencies)
    assert avg_latency <= 100, \
        f"Average camera latency {avg_latency:.1f}ms exceeds 100ms budget"

CI/CD for ROS2

# .github/workflows/ros2-tests.yml
name: ROS2 Tests

on:
  push:
    paths: ['src/**', 'tests/**']

jobs:
  unit-tests:
    runs-on: ubuntu-22.04
    container: ros:humble
    
    steps:
      - uses: actions/checkout@v4
      
      - name: Install dependencies
        run: |
          source /opt/ros/humble/setup.bash
          rosdep install --from-paths src --ignore-src -r -y
      
      - name: Build
        run: |
          source /opt/ros/humble/setup.bash
          colcon build --packages-select my_robot
      
      - name: Run unit tests
        run: |
          source /opt/ros/humble/setup.bash
          source install/setup.bash
          colcon test --packages-select my_robot
          colcon test-result --verbose
  
  simulation-tests:
    runs-on: ubuntu-22.04
    container: ros:humble-ros-base  # + Gazebo
    
    steps:
      - uses: actions/checkout@v4
      
      - name: Run simulation tests (headless)
        run: |
          source /opt/ros/humble/setup.bash
          export DISPLAY=:99
          Xvfb :99 -screen 0 1024x768x24 &
          pytest tests/test_gazebo_integration.py -v --timeout=120

Summary

ROS2 provides a full testing framework — most teams just don't use it:

  1. Unit test nodes — mock topics and services, test processing logic in isolation
  2. Launch tests — spin up real nodes and test their pub/sub interactions
  3. Simulation tests — full-stack validation in Gazebo, no hardware required
  4. Test timing — message rates and latencies are functional requirements, verify them
  5. CI/CD — run tests on every commit with GitHub Actions + ROS Docker containers

Robot software that isn't tested is robot software that fails in the field. Use HelpMeTest to schedule simulation regression tests and alert your team when navigation or perception regressions appear.

Read more

Start now free