dora-teleop/tests/test_deadman.py

"""The deadman: every way a source can go away, and the stop that follows it."""

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.joy import Sample
from tests.conftest import RecordingNode, input_event, joy_message, 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)


def send_joy(node, state, sample):
    """Deliver one reading the way `dora-joy` publishes it, decoding included."""
    loop._handle(node, state, input_event("joy", joy_message(sample)))


def ticks(node, state, count=1):
    """Run `count` ticks with nothing new arriving, and return every velocity sent."""
    for _ in range(count):
        loop._tick(node, state)
    return node.jogs()


def drive(node, state, sample, count=1):
    """Hold one reading for `count` ticks, the way a real pad and joy node would."""
    for _ in range(count):
        send_joy(node, state, sample)
        loop._tick(node, state)
    return node.jogs()


def web_jog(node, state, axes):
    """Deliver one console jog message, in the normalised units the page speaks."""
    loop._accept_ui_command(node, state, {"type": "jog", "axes": axes})


# --- 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 = build()

    jogs = drive(node, state, stick(axes=PUSHED))

    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 = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    jogs = drive(node, state, Sample(present=False))

    assert jogs == [STOP]


def test_the_joy_node_dying_stops_the_arm(clock):
    # The pad is another process now, and that process dying looks like silence
    # rather than like an unplugged pad. It has to mean the same thing: a
    # reading nobody is refreshing is not evidence that the stick is still held.
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    clock.advance(state.config.deadman.joy_ttl_s + 0.01)
    jogs = ticks(node, state)

    assert jogs == [STOP]


def test_a_joy_node_that_keeps_publishing_keeps_the_arm_moving(clock):
    # The mirror of the test above: time passing is not by itself a reason to
    # stop, or a held stick would stutter.
    node, state = build()

    sent = []
    for _ in range(4):
        clock.advance(0.05)
        sent = drive(node, state, stick(axes=PUSHED))

    assert sent
    assert all(jog[0] > 0 for jog in sent)


def test_a_stick_returning_to_centre_stops_the_arm(clock):
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    jogs = drive(node, state, stick())

    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 = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    jogs = drive(node, state, stick(), count=5)

    assert jogs == [STOP]


def test_a_held_stick_is_not_resent_every_tick(clock):
    # A velocity is a set-point the arm holds, not a frame in a sequence.
    # Repeating it every tick made the controller re-plan the same motion twenty
    # times a second, which an operator feels as the arm stepping rather than
    # travelling.
    node, state = build()

    jogs = drive(node, state, stick(axes=PUSHED), count=4)

    assert len(jogs) == 1
    assert jogs[0][0] > 0


def test_a_held_stick_is_repeated_before_the_arm_would_expire_it(clock):
    # The repeat exists for one reason: the arm's controller drops a velocity on
    # its own, and the repeat is what keeps that backstop from firing mid-motion.
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    clock.advance(state.config.deadman.jog_keepalive_s)
    jogs = drive(node, state, stick(axes=PUSHED))

    assert len(jogs) == 1
    assert jogs[0][0] > 0


def test_a_change_of_direction_never_waits_for_the_repeat(clock):
    # Sending only on change is worth nothing if the change itself is delayed:
    # a press or a release has to be acted on the tick it happens, not up to a
    # keepalive later.
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    jogs = drive(node, state, stick(axes=[0.0, 1.0]))

    assert len(jogs) == 1
    assert jogs[0][0] < 0


def test_a_pad_that_was_never_wired_up_leaves_the_arm_alone(clock):
    # `joy` is optional: a browser-only console wires no pad at all, and the
    # node must not read that as a centred stick asking for the arm.
    node, state = build()

    jogs = ticks(node, state, count=3)

    assert jogs == []
    assert state.arbiter.owner is None


# --- 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 = build()
    state.clients = 1
    state.arbiter.take(WEB, clock.t)
    web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
    ticks(node, state)
    node.clear()

    loop._set_clients(state, 0)
    jogs = ticks(node, state)

    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 = build()
    state.clients = 1
    state.arbiter.take(WEB, clock.t)
    web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
    ticks(node, state)
    node.clear()

    clock.advance(0.5)
    jogs = ticks(node, state)

    assert jogs == [STOP]


def test_a_browser_that_keeps_refreshing_keeps_the_arm_moving(clock):
    node, state = build()
    state.clients = 1
    state.arbiter.take(WEB, clock.t)

    sent = []
    for _ in range(4):
        clock.advance(0.15)
        web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
        loop._tick(node, state)
        sent = node.jogs()

    assert all(jog[0] == pytest.approx(state.config.speed_mm_s) for jog in sent)


def test_a_non_owner_jog_is_ignored(clock):
    node, state = build()
    state.clients = 1
    state.arbiter.take(GAMEPAD, clock.t)

    web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
    loop._tick(node, state)

    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)

    loop._accept_ui_command(node, state, {"type": "estop"})

    assert node.jogs() == [STOP]
    assert "stop" in node.services()


def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock):
    # A stop somebody had to reach for is not undone by the next thumb
    # movement. Without this the pad stops the arm for one tick and drives it
    # again on the next, which is not a stop at all.
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [True]))
    node.clear()
    jogs = drive(node, state, stick(axes=PUSHED), count=4)

    assert jogs == []


def test_the_pad_can_release_the_latch_it_set(clock):
    # A pad-only dataflow has no console RESET, so a latch nothing on the pad
    # can release would strand the arm until the process was restarted.
    node, state = build()
    drive(node, state, stick(buttons=[False] * 6 + [True]))
    assert state.view.estop is True
    node.clear()

    drive(node, state, stick(buttons=[False, False, True]))

    assert state.view.estop is False
    assert "clear" in node.services()


def test_the_latch_is_released_without_holding_the_arm(clock):
    # An idle pad hands the lease back two seconds after the stop it just made,
    # so gating the release on the lease would be a trap.
    node, state = build()
    drive(node, state, stick(buttons=[False] * 6 + [True]))
    state.arbiter.release(GAMEPAD)
    state.arbiter.take(WEB, clock.t)
    node.clear()

    drive(node, state, stick(buttons=[False, False, True]))

    assert state.view.estop is False


def test_the_arm_drives_again_once_the_latch_is_released(clock):
    node, state = build()
    drive(node, state, stick(buttons=[False] * 6 + [True]))
    drive(node, state, stick(buttons=[False, False, True]))
    node.clear()

    jogs = drive(node, state, stick(axes=PUSHED))

    assert jogs[-1][0] > 0


def test_a_latched_estop_stops_the_browser_too(clock):
    # The latch is the node's, not the pad's. A console jog must not walk
    # through a stop somebody made on the gamepad.
    node, state = build()
    state.clients = 1
    drive(node, state, stick(buttons=[False] * 6 + [True]))
    state.arbiter.release(GAMEPAD)
    state.arbiter.take(WEB, clock.t)
    node.clear()

    web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
    jogs = ticks(node, state)

    assert jogs == []


def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
    node, state = build()
    state.arbiter.take(WEB, clock.t)

    drive(node, state, stick(buttons=[False] * 6 + [True]))

    assert "stop" in node.services()


def test_a_latched_estop_is_cleared_by_reset(clock):
    node, state = build()
    state.view.estop = True

    loop._accept_ui_command(node, state, {"type": "reset"})

    assert state.view.estop is False
    assert "clear" in node.services()


# --- the standard ports ---


def test_the_same_deflection_from_either_source_asks_for_the_same_motion(clock):
    # The whole reason both sources are normalised before they are scaled. If
    # these ever differ, one path has grown a conversion the other has not —
    # which is how a console ends up in the robot's units.
    node, state = build()
    drive(node, state, stick(axes=[0.0, -1.0]))
    from_pad = node.twists()[-1]

    node, state = build()
    state.clients = 1
    state.arbiter.take(WEB, clock.t)
    web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
    ticks(node, state)
    from_web = node.twists()[-1]

    assert from_pad == from_web


def test_the_twist_is_si_and_the_legacy_jog_is_not(clock):
    # 60 mm/s is 0.06 m/s. A consumer reading the wrong port at the wrong scale
    # is a thousandfold error, so this is worth pinning.
    node, state = build()
    drive(node, state, stick(axes=[0.0, -1.0]))

    twist, jog = node.twists()[-1], node.jogs()[-1]

    assert twist["linear"][0] == pytest.approx(state.config.speed_mm_s / 1000.0)
    assert jog[0] == pytest.approx(state.config.speed_mm_s)
    assert twist["frame"] == "base"


def test_a_stop_reaches_both_ports(clock):
    node, state = build()
    drive(node, state, stick(axes=PUSHED))
    node.clear()

    drive(node, state, stick())

    assert node.jogs() == [STOP]
    assert node.twists() == [{"linear": [0.0] * 3, "angular": [0.0] * 3, "frame": "base"}]


def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
    # The verb is what a robot speaking the standard reads; the service name is
    # what dora-ulite6 reads until it does.
    node, state = build()

    drive(node, state, stick(buttons=[True]))

    assert node.actions() == ["gripper_close"]
    assert "grasp" in node.services()


def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock):
    node, state = build()
    drive(node, state, stick(buttons=[False, True]))
    drive(node, state, stick(buttons=[False] * 3 + [True]))
    drive(node, state, stick(buttons=[False] * 6 + [True]))
    drive(node, state, stick(buttons=[False, False, True]))

    assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]


def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock):
    # The lease has to be settled before the buttons are read. Read them first
    # and the press arrives while nobody owns the arm, and by the next tick the
    # button is held rather than newly down — so the edge is gone for good and
    # the operator presses A and watches nothing happen.
    node, state = build()
    assert state.arbiter.owner is None

    drive(node, state, stick(buttons=[True]), count=3)

    assert node.actions() == ["gripper_close"]