dora-teleop/tests/test_deadman.py

"""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()