Testing Robotics Code: 45 Tests Across Four Layers

A CI gate is only as trustworthy as the tests behind it. Forty-five tests run on every push, spread across four layers that go from "did you format your code" to "did the robot actually reach the goal in simulation." This post walks each layer, the bugs each one caught, and why the ordering matters.

The total at the time of writing: 45 tests, 0 failures, ~30 seconds of wall time for Layer 1, ~25 seconds of sim time for Layer 3. No test sleeps on a wall clock. No test depends on luck.

The four-layer pyramid

I deliberately did not build the classic three-tier Mike Cohn pyramid. Robotics has a layer cheaper than unit tests that catches a surprising number of bugs: linters and formatters, which run before any binary exists. Above that, pure-math unit tests validate the algorithms I was able to isolate. Above that, a smoke test confirms the system comes up at all. Above that, a single SIL acceptance scenario confirms the robot reaches a goal without hitting anything.

flowchart TB
    L0["Layer 0 — Linters<br/>ament_lint_auto · cpplint · xmllint · lint_cmake<br/>pre-commit (clang-format v18, YAML, &gt;1MB guard)<br/>~5 s · cheapest · earliest"]
    L1["Layer 1 — gtest unit tests<br/>13 tests · pure math · no sim · no ROS<br/>~30 s total · milliseconds each"]
    L2["Layer 2 — Smoke test<br/>1 test · 4 topics subscribed · /cmd_vel published<br/>conditional wait, 45 s ceiling · ~10 s typical"]
    L3["Layer 3 — SIL acceptance<br/>1 test · bridge + nav + rosbag · sim time<br/>~20 s sim · 120 s wall ceiling"]
    L0 --> L1 --> L2 --> L3
    style L0 fill:#1f3a5f,color:#fff
    style L1 fill:#2d5a8e,color:#fff
    style L2 fill:#3a7bb5,color:#fff
    style L3 fill:#4a9fde,color:#fff

Each layer is more expensive and slower than the one below. The pyramid is a fail-fast filter: a missing trailing newline in a YAML file should never cost you 20 seconds of sim time to discover. The order is enforced in the CI workflow — Layer 0 fails the job before Layer 1 is even compiled.

Layer 0 — Linters: cheapest, earliest, surprisingly effective

Every package declares ament_lint_auto with cpplint, xmllint, and lint_cmake. On top of that, a pinned pre-commit hook runs clang-format v18, strips trailing whitespace, validates YAML, and rejects any file larger than 1 MB — Hard Rule #5 from the project's contribution guide.

Linters have caught more real bugs than I expected. Two stand out:

• gtest include order. cpplint treats a .h angle-include as a C system header, which must come before <cmath>. I had them reversed in test_diff_drive_kinematics.cpp. The code compiled fine. cpplint refused to pass. The fix is one line; the bug would have survived review.
• Untracked files skipped by pre-commit --all-files. A new test file I added was not git add-ed yet, so pre-commit run --all-files silently ignored it. The hook looked green, the file was never formatted. The fix is procedural: git add before running the hook, or run pre-commit run --files <path> explicitly during development.

The >1 MB guard is not cosmetic. A rosbag or a PDF accidentally committed would balloon the repo and slow every clone. Pre-commit rejects it at the gate; git filter-repo never has to be invoked.

Layer 1 — gtest: 13 tests, pure math, no sim

This is the layer most robotics projects under-build. Thirteen gtest cases, all running in milliseconds, none touching ROS, MuJoCo, or a node. They validate functions that live in header-only pure-math modules, deliberately separated from the nodes that call them.

Testable architecture is a design choice, not luck. The kinematics, the go-to-goal controller, and the obstacle guard are header-only pure functions: (input) -> (output), no side effects, no node handles. The ROS nodes that wrap them are thin adapters that subscribe, call the function, and publish. The nodes have no unit tests because the nodes have no logic worth testing — the logic is in the functions.

This separation cost me maybe an afternoon of refactoring. It paid for itself the first time a unit test caught a sign error in the wrap-angle logic before it ever reached simulation.

Layer 2 — Smoke test: the silent QoS mismatch

test_odom_smoke.py is a single Python test that subscribes to four topics (/clock, /odom, /imu, /scan) and publishes /cmd_vel at 0.3 m/s. It uses a conditional wait, not a sleep. The test blocks on a condition variable that fires when all four subscribers have received at least one message. The only sleep in the file is a 45-second safety ceiling that exists solely to fail the test on a hang.

This is the test that caught the silent QoS mismatch. /imu and /scan are published with SensorDataQoS — best-effort, volatile, small depth. My first smoke subscriber used the default reliable QoS. Reliable subscriber + best-effort publisher = no messages, no error, no warning. The test would have waited 45 seconds and then failed with "no /imu received," and I would have stared at the publisher wondering why nothing was being sent.

The fix is one line: qos_profile_sensor_data on the subscriber. The lesson is structural: QoS mismatches are silent in ROS 2. A smoke test that subscribes to every topic the system is supposed to produce catches them at the gate instead of at the field.

Layer 3 — SIL acceptance: the scenario that matters

test_goal_reach.py is the one test that proves the robot does the thing the project exists to do. It spawns three processes: the MuJoCo bridge (with an obstacle scene), the nav stack, and a rosbag recorder. The test publishes a goal, then asserts on sim time, not wall clock, that the robot reaches the goal within tolerance, never gets closer than the clearance threshold to any obstacle, and finishes inside the sim-time budget.

flowchart LR
    B["MuJoCo bridge<br/>obstacle scene<br/>use_sim_time=false (clock source)"] -->|/clock /odom /imu /scan| N["nav stack<br/>use_sim_time=true"]
    N -->|/cmd_vel| B
    N -->|topics| R["rosbag record<br/>artifact on fail"]
    T["test_goal_reach.py<br/>use_sim_time=true"] -->|asserts on sim time| N
    T -->|goal pose| N
    style B fill:#2d5a8e,color:#fff
    style N fill:#3a7bb5,color:#fff
    style R fill:#5a3a8e,color:#fff
    style T fill:#4a9fde,color:#fff

The thresholds are the contract:

Asserting on sim time is non-negotiable. A slow CI runner should not fail a scenario that would pass on a fast one. The sim clock flows from the bridge, the nav stack runs on that clock, and the test asserts on that same clock. Wall time is only a ceiling to catch hangs.

This is the layer that caught the pitch bug — the one I'll cover in depth in Part 5. The short version: the robot pitches forward on acceleration, the front lidar beams dip below horizontal, hit the ground a meter ahead, and the obstacle guard reads them as a wall and stops. The fix is a front caster plus raising the velocity-feedback gain kv from 0.5 to 5.0 so the controller doesn't let the nose dive. No unit test could catch this — it's a physics-algorithm interaction. The kinematics are correct, the obstacle guard is correct, the controller is correct in isolation. Only the full SIL scenario, with a real lidar model and a real dynamic body, sees the coupling.

Test the tests: proving the gate isn't decorative

A green pipeline with tests that never fail is theater. I proved the gate blocks merges by deliberately breaking it. Two PRs closed as a result:

PR #8 is the important one. The failing run produced a 9567-message rosbag, uploaded as a downloadable CI artifact. I pulled it down, replayed it in RViz, watched the robot sit still for 20 sim seconds, and confirmed the diagnosis in five minutes. That is the workflow the gate is designed to enable: a failing scenario produces a forensic artifact, not just a red X.

These two PRs are the proof that the gate has teeth. If a broken test can merge, the tests are decoration. Mine can't.

Sim time philosophy

Wall-clock sleep is flaky on CI. Sim time plus conditional wait is deterministic. The clock flows from the simulation; the bridge is the source, and the source doesn't drink its own water — the bridge runs with use_sim_time=false because it is the clock. Every other process — nav, test, rosbag — runs with use_sim_time=true and consumes the bridge's /clock.

flowchart TD
    SIM["MuJoCo physics step"] --> BR["bridge node<br/>use_sim_time = false"]
    BR -->|publishes /clock| CLK["/clock topic"]
    BR -->|publishes /odom /imu /scan| TOPICS["sensor topics"]
    CLK --> NAV["nav stack<br/>use_sim_time = true"]
    CLK --> TEST["test_goal_reach.py<br/>use_sim_time = true"]
    CLK --> BAG["rosbag record<br/>use_sim_time = true"]
    TOPICS --> NAV
    NAV -->|publishes /cmd_vel| BR
    style BR fill:#2d5a8e,color:#fff
    style CLK fill:#5a3a8e,color:#fff
    style NAV fill:#3a7bb5,color:#fff
    style TEST fill:#4a9fde,color:#fff

The test does not time.sleep(20). It waits on a condition that fires when /odom reports the robot inside the goal tolerance, bounded by a 20-second sim-time deadline measured against /clock. On a fast machine this finishes in 8 wall seconds; on a slow CI runner it might take 40 wall seconds. Either way, the sim-time budget is identical: 20 seconds of simulated physics. The test is fair across hardware.

What this layering buys

The four layers cost roughly an afternoon to write and have caught, in order: a wrong include order, an untracked file, a silent QoS mismatch, and a physics-algorithm coupling that no isolated test could see. The cheap layers fail fast; the expensive layer fails with a forensic artifact. The total wall time of a green run is under two minutes.

The next post, Part 5, is about the bugs — the pitch bug in particular, how I found it in the rosbag, and why the fix had to span the mechanical model and the controller gains rather than living in any one layer.