A Deterministic Safety Gate for Differential-Drive Robots in ROS 2

I’ve been building a small supervisor node for differential-drive mobile robots that sits between a high-level velocity source (such as Nav2, an LLM agent, or teleoperation) and the base controller. Following the classic Simplex architecture (Sha et al.), the node treats complex high-level planners as unverified proposers and acts as a minimal, deterministic gatekeeper on /cmd_vel.

I work alone and I’m self-taught, based in Greece. I have no access to physical robot hardware, so everything described below is evaluated in Webots simulation (PAL Robotics TIAGo base) and verified in GitHub Actions Linux containers. I want to share what the node does, what the CI shows and doesn’t show, and the specific mistakes I made getting it through CI, in the hope it saves someone else an afternoon, or not

What it is

The package is qerra_core (node qerra_gate_node, version 0.3.0, Apache-2.0, ROS 2 Jazzy). It subscribes to /cmd_vel, /scan, and an operator reset topic, and publishes /cmd_vel_safe and diagnostic JSON on /gate/state.

planner (Nav2, teleop, etc.) --/cmd_vel–> [ qerra_gate_node ] --/cmd_vel_safe–> base controller
/scan ------^
distress input (UDP 5008) --^

The core decision logic is a pure function—step(state, params, inputs)—implemented using standard-library integer arithmetic with no external dependencies and with timestamps passed in as inputs. This design allows control cycles to be replayed and verified offline bit-for-bit.

Key behaviors:

1. Kinematic stopping envelope: Clamps wheel velocity commands against a swept forward lidar corridor:
   d_stop = ceil(v^2 / (2*a)) + ceil(v * t_react) + margin
   (where a = 0.5 m/s², t_react = 32 ms, margin = 300 mm).
2. Fail-closed trips: Commands zero velocity and latches on obstacle breach, stale/invalid proposals (≥ 150 ms), sensor stalls, or acute distress telemetry.
3. Calm dwell & operator reset: Once tripped, the robot remains stationary. Resuming motion strictly requires a continuous 1.0-second clear dwell window followed by an explicit operator reset message (/gate/reset). Reset is strictly refused while a hazard is present.
4. Distress reflex input (Layer 1B): Ingests non-blocking UDP telemetry (port 5008) inspired by ISO 13482 assistive cobot principles, tripping the gate on CRITICAL distress. (“HSR” in my documentation is my internal project name for this reflex layer and has no connection to Toyota’s robot).
5. Node-level fault latch: If an unhandled exception occurs inside the 32 ms control tick, the node latches into a permanent FAULT state, continually publishes a zero velocity command, reports the fault reason over /gate/state, and refuses to move until the process is restarted.
6. Topology note: The base controller is intended to be wired to /cmd_vel_safe, and nothing else should publish to it. ROS 2 topic namespaces do not enforce sole write authority.

What I can show (The Evidence)

• Simulation replays: Webots runs logging each tick with a SHA-256 rolling hash chain that verifies offline via verify_log.py --replay. In Demo 7, an obstacle trip fired at 337 mm clearance, reaching standstill at 301 mm (36 mm coasting). In Demo 9, injected human distress tripped the gate within the same 32 ms tick (cycle 125), held standstill through dwell stabilization, and motion resumed only after an authorized reset.
• Native ROS 2 Jazzy CI (workflow file: QERRA-v2-classical/.github/workflows/ros2_jazzy.yml at main · marunigno-ship-it/QERRA-v2-classical · GitHub Running on Ubuntu 24.04 in an official ros:jazzy container, GitHub Actions builds the package with colcon, verifies discovery in lib/qerra_core, starts the executable from a neutral directory (/tmp), captures a live /gate/state message, sends SIGINT, and strictly asserts that the process exits with code 0 and logs zero tick exceptions.

What I cannot show (Honesty Boundaries)

• No hardware validation: Everything is simulation and CI. A 0.0 velocity command is a software setpoint delivered to motor drivers, not a certified hardware Safe Torque Off (STO), brake coil, or emergency stop. If the gate process dies, the base controller will maintain its last command unless the base has its own hardware or software command timeout.
• Timing and jitter: The node runs on a Python timer at 31.25 Hz on general-purpose Linux. I have not profiled execution jitter under heavy system load.
• Reaction time (t_react): The 32 ms figure is a simulation assumption. On a real robot, total reaction latency includes physical sensor scan time (a Hokuyo URG-04LX operates at ~10 Hz / 100 ms), executor queueing, DDS transport, and motor deceleration ramps. Until measured, d_stop is likely optimistic for real hardware.
• Comparison with Nav2: I have not benchmarked this against nav2_collision_monitor. nav2_collision_monitor is far more mature and feature-complete. My implementation is an exploration of Simplex state-latching, explicit calm dwells, and replayable pure-function cores.
• Standards are informative: References to ISO 13849-1, ISO 13482, and ISO 3691-4 are conceptual references only; no formal compliance is claimed.

What went wrong in CI (Lessons Learned)

Getting the standalone node to build, tick, and terminate cleanly under native Jazzy CI exposed five subtle issues:

1. Missing setup.cfg: The console script was initially installed into bin/ rather than lib//, leaving ros2 pkg executables completely blank. Adding setup.cfg with explicit script directories fixed discovery.
2. False confidence from GNU timeout: I initially ran the node under timeout -s INT 5 and treated exit code 124 as proof of clean operation. However, GNU timeout returns 124 whenever the time expires, regardless of whether the child process handled SIGINT cleanly or hung. I switched to launching in the background, issuing kill -INT, and checking the child’s actual exit status.
3. The SIGINT shutdown crash: The actual exit status turned out to be 1. In ROS 2 Jazzy, rclpy.spin() raises ExternalShutdownException upon SIGINT. My finally: block was calling rclpy.shutdown() unconditionally on a context that had already been shut down, throwing an RCLError via pybind11. The fix was catching ExternalShutdownException and guarding if rclpy.ok(): rclpy.shutdown().
4. Swallowed tick exceptions: Wrapping the control loop in except Exception: and publishing zero commands masked runtime failures during CI smoke testing. The step now inspects the log to fail the build if any “Tick exception” occurred, and the node now latches fail-closed.
5. Sensor beam count mismatch: The Webots TIAGo lidar outputs 667 samples, while a physical Hokuyo URG-04LX outputs 682. My first iteration auto-configured the gate parameters from the first scan received—which made length validation useless because it compared the scan against itself. Geometry parameters are now immutable at startup, and mismatched scans are discarded (TRIP_SENSOR_INVALID).

Where this sits

This supervisory gate is the physical foundation (Layer 1) of a broader research framework. A text-based moral filter (SEMEV-12) and a values-based action ranker (QERRA-THRIVE) sit upstream in deliberation; they are purely advisory and do not form part of the safety function.

Repository & Documentation: GitHub - marunigno-ship-it/QERRA-v2-classical: Three-layer deterministic safety & ethical execution guard for autonomous robots (ROS 2 & Behavior Trees). Sub-ms physical reflex (QERRA-HSR) + moral filter (SEMEV-12) + values ranker (THRIVE). Fully auditable. · GitHub

Complete README: QERRA-v2-classical/README.md at main · marunigno-ship-it/QERRA-v2-classical · GitHub

Questions for the Community

1. Bounding reaction latency: How do you measure and bound worst-case end-to-end reaction time in ROS 2 (sensor packet arrival → executor dispatch → supervisor decision → base controller command)? Is ros2_tracing the standard tool for this, and what instrumentation points would you recommend?
2. Integration topology: For safety gatekeepers on /cmd_vel_safe, do you prefer direct topic remapping to the motor base, or integrating the supervisor into a twist_mux priority hierarchy?
3. Lifecycle node management: Should a supervisory gate of this type be structured as a LifecycleNode? If so, how do you model a persistent “fault latched until restart” state without an external lifecycle manager inadvertently resetting the node before inspection?
4. Benchmarking with nav2_collision_monitor: What would a fair, reproducible evaluation between a Simplex-style state machine and nav2_collision_monitor look like (e.g., standard bag replays, deceleration envelope metrics)?

I built this with AI coding assistance (documented in the README) and verified every claim against test suites (192 unit tests passing) and CI logs. If you spot an error, architectural blind spot, or flawed assumption, please let me know.