Robotics Testing

Robotics Testing Skill

When to Use This Skill

• Writing unit tests for ROS1/ROS2 nodes
• Setting up integration tests with launch_testing
• Mocking hardware (sensors, actuators) for CI/CD
• Building simulation-based test suites
• Testing perception pipelines with ground truth
• Validating trajectory planners and controllers
• Setting up CI/CD pipelines for robotics packages
• Debugging flaky tests in robotics systems

The Robotics Testing Pyramid

                    ╱╲
                   ╱  ╲        Field Tests
                  ╱    ╲       (Real robot, real environment)
                 ╱──────╲
                ╱        ╲     Hardware-in-the-Loop (HIL)
               ╱          ╲    (Real hardware, controlled environment)
              ╱────────────╲
             ╱              ╲   Simulation Tests
            ╱                ╲  (Full sim, realistic physics)
           ╱──────────────────╲
          ╱                    ╲  Integration Tests
         ╱                      ╲ (Multi-node, message passing)
        ╱────────────────────────╲
       ╱                          ╲ Unit Tests
      ╱____________________________╲ (Single function/class, fast, deterministic)

MORE tests at the bottom, FEWER at the top.
Bottom = fast, cheap, deterministic. Top = slow, expensive, realistic.

Unit Testing Patterns

Testing ROS2 Nodes with pytest

# test_perception_node.py
import pytest
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from my_pkg.perception_node import PerceptionNode
import numpy as np

@pytest.fixture(scope='module')
def ros_context():
    """Initialize ROS2 context once per test module"""
    rclpy.init()
    yield
    rclpy.shutdown()

@pytest.fixture
def perception_node(ros_context):
    """Create a fresh perception node for each test"""
    node = PerceptionNode()
    yield node
    node.destroy_node()

@pytest.fixture
def test_image():
    """Generate a synthetic test image"""
    msg = Image()
    msg.height = 256
    msg.width = 256
    msg.encoding = 'rgb8'
    msg.step = 256 * 3
    msg.data = np.random.randint(0, 255, (256, 256, 3),
                                  dtype=np.uint8).tobytes()
    return msg

class TestPerceptionNode:

    def test_initialization(self, perception_node):
        """Node should initialize with correct default parameters"""
        assert perception_node.get_parameter('confidence_threshold').value == 0.7
        assert perception_node.get_parameter('rate_hz').value == 30.0

    def test_parameter_validation(self, perception_node):
        """Node should reject invalid parameter values"""
        from rcl_interfaces.msg import SetParametersResult
        result = perception_node.set_parameters([
            rclpy.parameter.Parameter('confidence_threshold',
                                       value=-0.5)  # Invalid!
        ])
        assert not result[0].successful

    def test_image_callback_publishes_detections(self, perception_node, test_image):
        """Processing an image should produce detection output"""
        received = []

        # Create a test subscriber
        sub_node = Node('test_subscriber')
        sub_node.create_subscription(
            DetectionArray, '/perception/detections',
            lambda msg: received.append(msg), 10)

        # Simulate image callback
        perception_node.image_callback(test_image)

        # Spin briefly to allow message propagation
        rclpy.spin_once(sub_node, timeout_sec=1.0)
        rclpy.spin_once(perception_node, timeout_sec=1.0)

        # Verify
        assert len(received) > 0
        sub_node.destroy_node()

    def test_empty_image_handling(self, perception_node):
        """Node should handle empty/corrupted images gracefully"""
        empty_msg = Image()  # No data
        # Should not crash
        perception_node.image_callback(empty_msg)

Testing Pure Functions (No ROS Dependency)

# test_kinematics.py
import pytest
import numpy as np
from my_pkg.kinematics import (
    forward_kinematics, inverse_kinematics,
    quaternion_multiply, transform_point
)

class TestForwardKinematics:

    @pytest.mark.parametrize("joint_angles,expected_pos", [
        # Home position
        (np.zeros(7), np.array([0.088, 0.0, 1.033])),
        # Known calibrated pose
        (np.array([0, -0.785, 0, -2.356, 0, 1.571, 0.785]),
         np.array([0.307, 0.0, 0.59])),
    ])
    def test_known_poses(self, joint_angles, expected_pos):
        """FK should match known calibrated positions"""
        result = forward_kinematics(joint_angles)
        np.testing.assert_allclose(result[:3], expected_pos, atol=0.01)

    def test_fk_ik_roundtrip(self):
        """FK(IK(pose)) should return the original pose"""
        original_pose = np.array([0.4, 0.1, 0.5, 1.0, 0.0, 0.0, 0.0])
        joint_angles = inverse_kinematics(original_pose)
        recovered_pose = forward_kinematics(joint_angles)
        np.testing.assert_allclose(recovered_pose, original_pose, atol=1e-4)

    def test_joint_limits_respected(self):
        """IK should not return angles outside joint limits"""
        target = np.array([0.5, 0.2, 0.3, 1.0, 0.0, 0.0, 0.0])
        joints = inverse_kinematics(target)
        for i, (lo, hi) in enumerate(JOINT_LIMITS):
            assert lo  obs.radius, \
                    f"Collision at {point[:3]}, dist={dist:.4f}"

Simulation Testing

class SimulationTestHarness:
    """Run behavior tests in simulation with deterministic physics"""

    def __init__(self, sim_config):
        self.sim = MuJoCoSimulator(sim_config)
        self.sim.set_seed(42)  # Deterministic physics

    def test_pick_and_place(self):
        """Full pick-and-place task in simulation"""
        # Setup scene
        self.sim.reset()
        self.sim.spawn_object('red_block', pose=[0.4, 0.1, 0.02])

        # Run behavior tree
        bt = create_pick_place_tree()
        bt.setup(sim=self.sim)

        max_steps = 1000
        for step in range(max_steps):
            bt.tick()
            self.sim.step()

            if bt.root.status == Status.SUCCESS:
                break

        # Verify outcome
        block_pose = self.sim.get_object_pose('red_block')
        target_pose = np.array([0.5, -0.1, 0.02])
        assert np.linalg.norm(block_pose[:3] - target_pose)  0

Source & license

This open-source skill is cataloged on AgentStack and links to its original source — we do not rehost the code.

• Author: arpitg1304
• Source: arpitg1304/robotics-agent-skills
• License: Apache-2.0

Install and usage instructions live in the source repository linked above.