add the event loop and the deadman

The loop owns the one decision this node exists to make: whether a velocity
reaches the arm this tick, and from whom.

An arm under a velocity command keeps moving until told otherwise, so the
question is never whether to start but whether anything has happened that means
it must stop. Every path through _wanted_velocity that is not an owner actively
asking for motion returns a stop, so a case nobody thought of fails by halting
rather than by carrying on. The default has to be still.

Three ways a source goes away, all ending in the same place. It goes quiet, and
the watchdog fires after JOG_TTL_S. It goes away outright — the pad unplugged,
the last browser gone — and the arm stops at once, without waiting out the
timer, because an unplugged pad is not a centred pad. Or the process itself
goes away, and one all-zero velocity is sent before it does: a motion lives in
the controller, not here.

The stop is sent once, not every tick. A stream of zeroes would keep the arm
node believing a jog is live and hold robot_state at moving forever.

E-STOP is not gated on the lease, from either source. Somebody reaching for the
stop is the one moment when asking who owns the arm is the wrong question.
This commit is contained in:
2026-09-15 15:43:06 -05:00
parent 9f56b2b334
commit 56b86699bd
2 changed files with 580 additions and 0 deletions
+215
View File
@@ -0,0 +1,215 @@
"""The deadman: every way a source can go away, and the stop that follows it."""
import json
import pyarrow as pa
import pytest
from dora_teleop import main as loop
from dora_teleop.arbiter import GAMEPAD, WEB
from dora_teleop.config import Config
from dora_teleop.gamepad import Sample
from tests.conftest import FakeGamepad, RecordingNode, input_event, stick
STOP = [0.0] * 6
PUSHED = [0.0, -1.0]
@pytest.fixture
def clock(monkeypatch):
"""Return a clock the test moves by hand, so no test waits out a real timeout."""
class Clock:
def __init__(self):
self.t = 1000.0
def advance(self, seconds):
self.t += seconds
moved = Clock()
monkeypatch.setattr("dora_teleop.main.now", lambda: moved.t)
return moved
def build():
config = Config.from_env()
config.expo = 0.0
return RecordingNode(), loop.Teleop(config), FakeGamepad()
def jog_from(node, state, gamepad, count=1):
"""Run `count` ticks and return every velocity that reached the arm."""
for _ in range(count):
loop._tick(node, state, gamepad)
return node.jogs()
def web_jog(node, state, velocity):
"""Deliver one console jog message."""
payload = json.dumps({"type": "jog", "velocity": velocity})
loop._accept_ui_command(node, state, input_event("ui_command", pa.array([payload])))
# --- the gamepad ---
def test_a_pushed_stick_takes_the_arm_and_drives_it(clock):
# Nobody should have to reach for a browser to use the pad in their hands.
node, state, gamepad = build()
gamepad.sample = stick(axes=PUSHED)
jogs = jog_from(node, state, gamepad)
assert state.arbiter.owner == GAMEPAD
assert jogs[-1][0] > 0
def test_a_gamepad_that_is_unplugged_mid_jog_stops_the_arm(clock):
# An unplugged pad is not a centred pad, and waiting out the TTL here is the
# difference between a dropped controller costing 10 ms of travel and 400.
node, state, gamepad = build()
gamepad.sample = stick(axes=PUSHED)
jog_from(node, state, gamepad)
node.clear()
gamepad.sample = Sample(present=False)
jogs = jog_from(node, state, gamepad)
assert jogs == [STOP]
def test_a_stick_returning_to_centre_stops_the_arm(clock):
node, state, gamepad = build()
gamepad.sample = stick(axes=PUSHED)
jog_from(node, state, gamepad)
node.clear()
gamepad.sample = stick()
jogs = jog_from(node, state, gamepad)
assert jogs == [STOP]
def test_a_stick_at_rest_sends_one_zero_not_a_stream(clock):
# A stream of zeroes would keep the arm node believing a jog is live and
# hold robot_state at `moving` forever.
node, state, gamepad = build()
gamepad.sample = stick(axes=PUSHED)
jog_from(node, state, gamepad)
node.clear()
gamepad.sample = stick()
jogs = jog_from(node, state, gamepad, count=5)
assert jogs == [STOP]
def test_a_held_stick_is_resent_every_tick(clock):
# The arm's controller expires a velocity on its own; the re-send is what
# keeps that backstop from firing mid-motion.
node, state, gamepad = build()
gamepad.sample = stick(axes=PUSHED)
jogs = jog_from(node, state, gamepad, count=4)
assert len(jogs) == 4
assert all(jog[0] > 0 for jog in jogs)
# --- the browser ---
def test_the_last_browser_leaving_stops_the_arm(clock):
# Closing the tab has to stop the arm, and nothing is sent when it does.
node, state, gamepad = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
jog_from(node, state, gamepad)
node.clear()
loop._accept_clients(state, input_event("clients", pa.array([{"connected": 0, "event": "close"}])))
jogs = jog_from(node, state, gamepad)
assert jogs == [STOP]
assert state.arbiter.owner is None
def test_a_browser_that_goes_quiet_stops_the_arm(clock):
# The console refreshes every 150 ms against a 400 ms deadline.
node, state, gamepad = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
jog_from(node, state, gamepad)
node.clear()
clock.advance(0.5)
jogs = jog_from(node, state, gamepad)
assert jogs == [STOP]
def test_a_browser_that_keeps_refreshing_keeps_the_arm_moving(clock):
node, state, gamepad = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
sent = []
for _ in range(4):
clock.advance(0.15)
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
loop._tick(node, state, gamepad)
sent = node.jogs()
assert all(jog[0] == pytest.approx(10.0) for jog in sent)
def test_a_non_owner_jog_is_ignored(clock):
node, state, gamepad = build()
state.clients = 1
state.arbiter.take(GAMEPAD, clock.t)
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
loop._tick(node, state, gamepad)
assert node.jogs() == []
assert any(message.get("type") == "error" for message in node.ui())
# --- the stop that is never gated ---
def test_estop_is_never_gated_by_the_lease(clock):
# Somebody reaching for the stop is the one moment when asking who owns the
# arm is the wrong question.
node, state, _ = build()
state.clients = 1
state.arbiter.take(GAMEPAD, clock.t)
payload = json.dumps({"type": "estop"})
loop._accept_ui_command(node, state, input_event("ui_command", pa.array([payload])))
assert node.jogs() == [STOP]
assert "stop" in node.services()
def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
node, state, gamepad = build()
state.arbiter.take(WEB, clock.t)
gamepad.sample = stick(buttons=[False] * 6 + [True])
loop._tick(node, state, gamepad)
assert "stop" in node.services()
def test_a_latched_estop_is_cleared_by_reset(clock):
node, state, _ = build()
state.view.estop = True
payload = json.dumps({"type": "reset"})
loop._accept_ui_command(node, state, input_event("ui_command", pa.array([payload])))
assert state.view.estop is False
assert "clear" in node.services()