dora_teleop/main.py

@@ -0,0 +1,365 @@
+"""The dora event loop: routing, the deadman, and nothing that can block.
+
+The loop owns the one decision this node exists to make — whether a velocity
+reaches the arm this tick, and from whom. Everything else is delegated:
+:mod:`dora_teleop.gamepad` owns the hardware, :mod:`dora_teleop.mapping` turns a
+reading into a velocity, :mod:`dora_teleop.arbiter` says who is allowed to, and
+:mod:`dora_teleop.ui` speaks the console's language.
+
+**The safety-critical part is `_drive`.** An arm moving under a velocity command
+keeps moving until told otherwise, so the question is never "should it start"
+but "has anything happened that means it must stop". Three things mean that: the
+source went quiet, the source went away, or this process is going away. All
+three end in the same place — one all-zero velocity, sent once.
+"""
+
+import argparse
+import json
+import time
+
+import pyarrow as pa
+from dora import Node
+
+from dora_teleop import mapping, ui
+from dora_teleop.arbiter import GAMEPAD, WEB, Arbiter
+from dora_teleop.config import Config
+from dora_teleop.gamepad import Gamepad, now
+from dora_teleop.log import log
+from dora_teleop.mapping import LAYOUT, pressed
+
+REQUEST_ID = "request_id"
+STOP_VELOCITY = [0.0] * 6
+
+
+class Teleop:
+    """Everything the loop remembers between ticks."""
+
+    def __init__(self, config: Config):
+        self.config = config
+        self.arbiter = Arbiter(config.deadman.gamepad_idle_release_s)
+        self.view = ui.ArmView()
+        # The velocity the browser last asked for, and when. A jog is a stream;
+        # what matters is whether the stream is still arriving.
+        self.web_velocity = STOP_VELOCITY
+        self.web_seen_at = 0.0
+        self.clients = 0
+        # Whether the arm is currently being driven by us. Used to send the stop
+        # once rather than every tick: a stream of zeroes would keep the arm
+        # node believing a jog is live and hold `robot_state` at `moving`.
+        self.jogging = False
+        self.buttons: list[bool] = []
+        self.requests = 0
+        self.polled_at = 0.0
+
+    def next_request_id(self, service: str) -> str:
+        """Mint a correlation id we will recognise coming back."""
+        self.requests += 1
+        return f"teleop-{service}-{self.requests}"
+
+
+def main() -> None:
+    """Connect to the dataflow, start reading the pad, and run until dora stops us."""
+    args = _parse_args()
+    config = Config.from_env()
+
+    # Passing the name lets the node also run standalone as a dynamic node; when
+    # dora spawns it, the id from the dataflow wins.
+    node = Node(args.name)
+    state = Teleop(config)
+    gamepad = Gamepad(config.joystick_index)
+    gamepad.start()
+
+    try:
+        for event in node:
+            if event["type"] == "ERROR":
+                raise RuntimeError(event["error"])
+            if event["type"] != "INPUT":
+                continue
+            _handle(node, state, gamepad, event)
+    finally:
+        # A motion lives in the arm's controller, not in this process. Leaving
+        # without zeroing the velocity leaves it travelling until the
+        # controller's own expiry catches up, which is the one moment nobody is
+        # watching for it.
+        _send_jog(node, STOP_VELOCITY)
+        gamepad.shutdown()
+
+
+def _parse_args() -> argparse.Namespace:
+    """Read the node name, for when dora did not spawn us."""
+    parser = argparse.ArgumentParser(description="Drive a robot arm from a gamepad or a browser.")
+    parser.add_argument(
+        "--name",
+        default="teleop",
+        help="The name of the node in the dataflow, used when running as a dynamic node.",
+    )
+    return parser.parse_args()
+
+
+def _handle(node: Node, state: Teleop, gamepad: Gamepad, event: dict) -> None:
+    """Route one input to whatever deals with it."""
+    event_id = event["id"]
+    if event_id == "tick":
+        _tick(node, state, gamepad)
+    elif event_id == "ui_command":
+        _accept_ui_command(node, state, event)
+    elif event_id == "clients":
+        _accept_clients(state, event)
+    elif event_id == "robot_state":
+        state.view.state = event["value"][0].as_py()
+    elif event_id == "tcp_pose":
+        state.view.pose = event["value"].to_pylist()
+    elif event_id == "joint_pose":
+        state.view.joints = event["value"].to_pylist()
+    elif event_id == "response":
+        _accept_response(node, state, event)
+    elif event_id == "motion_state":
+        _accept_motion_state(node, state, event)
+    else:
+        log(f"ignoring unexpected input `{event_id}`")
+
+
+# --- the tick, where the arm is actually driven ---
+
+
+def _tick(node: Node, state: Teleop, gamepad: Gamepad) -> None:
+    """Decide what the arm should be doing, and say so."""
+    moment = now()
+    sample = gamepad.read()
+
+    _read_buttons(node, state, sample)
+
+    # A pad taking the arm the moment a stick moves is what keeps an operator
+    # from having to reach for a browser to use the thing in their hands.
+    if sample.active and state.arbiter.owner is None:
+        state.arbiter.take(GAMEPAD, moment)
+    if sample.active:
+        state.arbiter.note_activity(GAMEPAD, moment)
+    state.arbiter.expire_idle_gamepad(moment)
+
+    _drive(node, state, sample, moment)
+    _poll_state(node, state, moment)
+    _publish(node, state)
+
+
+def _drive(node: Node, state: Teleop, sample, moment: float) -> None:
+    """Send the velocity the owner is asking for, or the stop that ends it.
+
+    A velocity is only re-sent while it is non-zero: the arm node treats a
+    stream of jogs as one continuous one, and the re-send exists so the
+    controller's own expiry never fires mid-motion.
+    """
+    velocity = _wanted_velocity(state, sample, moment)
+
+    if mapping.is_moving(velocity):
+        state.jogging = True
+        _send_jog(node, velocity)
+        return
+
+    if state.jogging:
+        # Once, not every tick. Repeating it would keep the arm node believing a
+        # jog is live and hold `robot_state` at `moving` forever.
+        state.jogging = False
+        _send_jog(node, STOP_VELOCITY)
+
+
+def _wanted_velocity(state: Teleop, sample, moment: float) -> list[float]:
+    """Return what the arm should be doing, which is usually nothing.
+
+    Every path that is not an owner actively asking for motion returns a stop.
+    That is deliberate: the default has to be "still", so that a case nobody
+    thought of fails by halting rather than by carrying on.
+    """
+    owner = state.arbiter.owner
+    if owner == GAMEPAD:
+        # An unplugged pad is not a centred pad. Stopping here rather than
+        # waiting out the TTL is the difference between a dropped controller
+        # costing 10 ms of travel and costing 400.
+        if not sample.present:
+            return STOP_VELOCITY
+        return mapping.velocity(sample.axes, sample.buttons, state.config)
+
+    if owner == WEB:
+        # The console refreshes its jog every 150 ms; the browser going quiet,
+        # or the tab closing, has to stop the arm without anything being sent.
+        if state.clients == 0:
+            return STOP_VELOCITY
+        if moment - state.web_seen_at > state.config.deadman.jog_ttl_s:
+            return STOP_VELOCITY
+        return state.web_velocity
+
+    return STOP_VELOCITY
+
+
+def _read_buttons(node: Node, state: Teleop, sample) -> None:
+    """Fire the services a gamepad button asks for, on the press only.
+
+    Edge-triggered, because these are one-shot requests rather than a held
+    velocity: holding `grasp` should not send a request every 50 ms.
+    """
+    previous, state.buttons = state.buttons, list(sample.buttons)
+    if not sample.present:
+        return
+
+    def tapped(index: int) -> bool:
+        return pressed(sample.buttons, index) and not pressed(previous, index)
+
+    # The stop is not gated on the lease. Somebody reaching for it is the one
+    # moment when asking who owns the arm is the wrong question.
+    if tapped(LAYOUT.estop):
+        _estop(node, state)
+    if not state.arbiter.may_drive(GAMEPAD):
+        return
+    if tapped(LAYOUT.grasp):
+        _request(node, state, {"service": "grasp"})
+    if tapped(LAYOUT.release):
+        _request(node, state, {"service": "release"})
+    if tapped(LAYOUT.clear):
+        _request(node, state, {"service": "clear"})
+    if tapped(LAYOUT.freedrive):
+        _request(node, state, {"service": "freedrive", "enabled": not state.view.freedrive_on})
+
+
+def _poll_state(node: Node, state: Teleop, moment: float) -> None:
+    """Ask the arm for what telemetry does not carry.
+
+    Whether the tool is engaged and whether freedrive is on only come back from
+    `get_state`, which the arm node answers off its report cache and so can
+    answer during a motion.
+    """
+    if moment - state.polled_at < state.config.deadman.state_poll_s:
+        return
+    state.polled_at = moment
+    _request(node, state, {"service": "get_state"})
+
+
+# --- what the console asks for ---
+
+
+def _accept_ui_command(node: Node, state: Teleop, event: dict) -> None:
+    """Take one console message and do what it says."""
+    raw = event["value"][0].as_py() if len(event["value"]) else None
+    if raw is None:
+        return
+    try:
+        payload = json.loads(raw) if isinstance(raw, str) else raw
+    except json.JSONDecodeError:
+        log("ignoring a console message that was not JSON")
+        return
+
+    command = ui.command_of(payload)
+    if command is None:
+        return
+    moment = now()
+
+    if command.type == "estop":
+        _estop(node, state)
+        return
+    if command.type == "control":
+        _control(node, state, command, moment)
+        return
+    if command.type == "reset":
+        # Clearing the latch is this node's business; clearing the controller's
+        # error is the arm's. Both, in that order, or the console's RESET button
+        # leaves one of the two still saying stop.
+        state.view.estop = False
+        _request(node, state, {"service": "clear"})
+        return
+
+    if not state.arbiter.may_drive(WEB):
+        _post_ui(node, ui.error("take control first"))
+        return
+
+    if command.type == "jog":
+        state.web_velocity = command.velocity
+        state.web_seen_at = moment
+        state.arbiter.note_activity(WEB, moment)
+    elif command.type == "jog_stop":
+        state.web_velocity = STOP_VELOCITY
+        state.web_seen_at = moment
+    elif command.type == "home":
+        _request(node, state, {"service": "home", "wait": False})
+    elif command.type == "gripper":
+        _request(node, state, {"service": "grasp" if command.enabled else "release"})
+    elif command.type == "freedrive":
+        _request(node, state, {"service": "freedrive", "enabled": command.enabled})
+    elif command.type == "move":
+        _request(node, state, {"service": "move_to", "wait": False, **command.row})
+
+
+def _control(node: Node, state: Teleop, command, moment: float) -> None:
+    """Hand the arm to the console, or take it back."""
+    if command.action == "release":
+        state.arbiter.release(WEB)
+        state.web_velocity = STOP_VELOCITY
+        return
+    if not state.arbiter.take(WEB, moment, preempt=command.preempt):
+        _post_ui(node, ui.error(f"the {state.arbiter.owner} has the arm"))
+
+
+def _accept_clients(state: Teleop, event: dict) -> None:
+    """Track who is watching, because the last one leaving has to stop the arm."""
+    row = event["value"][0].as_py()
+    state.clients = int(row.get("connected", 0))
+    if state.clients == 0:
+        state.arbiter.release(WEB)
+        state.web_velocity = STOP_VELOCITY
+
+
+# --- what the arm says back ---
+
+
+def _accept_response(node: Node, state: Teleop, event: dict) -> None:
+    """Absorb one answer, and tell the console about it if it was bad news."""
+    row = event["value"][0].as_py()
+    if row.get("service") == "get_state":
+        state.view.take_get_state(row)
+        return
+    if not row.get("ok", True):
+        message = row.get("message", "")
+        log(f"{row.get('service')} failed: {message}")
+        _post_ui(node, ui.error(message))
+
+
+def _accept_motion_state(node: Node, state: Teleop, event: dict) -> None:
+    """Report a motion that failed, which a non-blocking request learns no other way."""
+    row = event["value"][0].as_py()
+    if row.get("state") == "failed":
+        _post_ui(node, ui.error(row.get("message", "the motion failed")))
+
+
+# --- talking to the dataflow ---
+
+
+def _estop(node: Node, state: Teleop) -> None:
+    """Halt the arm, from any source, held lease or not."""
+    log("e-stop")
+    state.view.estop = True
+    state.jogging = False
+    state.web_velocity = STOP_VELOCITY
+    _send_jog(node, STOP_VELOCITY)
+    _request(node, state, {"service": "stop"})
+
+
+def _send_jog(node: Node, velocity: list[float]) -> None:
+    """Send one velocity to the arm. There is no answer to a jog, ever."""
+    node.send_output("jog", pa.array([{"velocity": [float(v) for v in velocity]}]))
+
+
+def _request(node: Node, state: Teleop, row: dict) -> None:
+    """Call one service on the arm under an id we will recognise coming back."""
+    node.send_output("request", pa.array([row]), metadata={REQUEST_ID: state.next_request_id(row["service"])})
+
+
+def _publish(node: Node, state: Teleop) -> None:
+    """Send the console its picture of the arm."""
+    _post_ui(node, ui.snapshot(state.view, state.arbiter.owner, state.jogging, time.time()))
+
+
+def _post_ui(node: Node, payload: dict) -> None:
+    """Hand one object to the bridge, which broadcasts it to every browser."""
+    node.send_output("ui_state", pa.array([json.dumps(payload)]), metadata={"encoding": "json"})
+
+
+if __name__ == "__main__":
+    main()

tests/test_deadman.py

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