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:
@@ -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()
|
||||
Reference in New Issue
Block a user