dora_teleop/arbiter.py

@@ -0,0 +1,78 @@
+"""Who is allowed to drive the arm.
+
+Two sources want one arm, and the arm cannot take both. This is the whole of
+that decision, in one place, so there is exactly one answer to "why did it not
+move" rather than one per input path.
+
+The model comes from the monolithic repo's `RobotService` lease, and is kept
+because the old web console is built on it: its jog pad is gated on owning the
+arm and it renders an owner chip. A simpler last-writer-wins scheme would mean
+rewriting the console to match the node rather than the other way round.
+
+The arm node itself has no idea any of this exists. A lease held here binds only
+what goes through this node — another node publishing `request` to the arm will
+preempt an operator mid-motion, and nothing here can stop it.
+"""
+
+from dora_teleop.log import log
+
+GAMEPAD = "gamepad"
+WEB = "web"
+
+
+class Arbiter:
+    """The lease on the arm, and the rules for handing it between sources."""
+
+    def __init__(self, idle_release_s: float):
+        self.idle_release_s = idle_release_s
+        self._owner: str | None = None
+        self._last_activity = 0.0
+
+    @property
+    def owner(self) -> str | None:
+        """Who holds the arm, or None if it is free."""
+        return self._owner
+
+    def take(self, who: str, now: float, preempt: bool = False) -> bool:
+        """Give ``who`` the arm if it is theirs to take, and say whether it worked.
+
+        A free arm goes to whoever asks. A held one stays held unless the caller
+        asked to preempt, which the console's TAKE CONTROL button offers
+        deliberately: an operator who can see the arm is better placed than this
+        code to decide they should have it.
+        """
+        if self._owner not in (None, who) and not preempt:
+            return False
+        if self._owner != who:
+            log(f"{who} has the arm" + (f", preempting {self._owner}" if self._owner else ""))
+        self._owner = who
+        self._last_activity = now
+        return True
+
+    def release(self, who: str) -> None:
+        """Give the arm back, if this source had it."""
+        if self._owner == who:
+            log(f"{who} released the arm")
+            self._owner = None
+
+    def note_activity(self, who: str, now: float) -> None:
+        """Record that the holder is still there, so the idle timer does not fire."""
+        if self._owner == who:
+            self._last_activity = now
+
+    def expire_idle_gamepad(self, now: float) -> None:
+        """Hand the arm back after the pad has sat untouched long enough.
+
+        Only the gamepad expires. It takes the arm implicitly, on the first
+        nudge of a stick, so it has to give it back the same way or a pad left
+        on the bench would lock the browser out forever. The web takes the arm
+        by pressing a button and gives it back by pressing another.
+        """
+        if self._owner != GAMEPAD:
+            return
+        if now - self._last_activity >= self.idle_release_s:
+            self.release(GAMEPAD)
+
+    def may_drive(self, who: str) -> bool:
+        """Whether this source's jog should reach the arm."""
+        return self._owner == who

tests/test_arbiter.py

@@ -0,0 +1,106 @@
+"""The lease: who may drive the arm, and how it changes hands."""
+
+from dora_teleop.arbiter import GAMEPAD, WEB, Arbiter
+
+
+def build(idle_release_s=2.0):
+    return Arbiter(idle_release_s)
+
+
+def test_a_free_arm_goes_to_whoever_asks():
+    arbiter = build()
+
+    assert arbiter.take(WEB, now := 100.0) is True
+    assert arbiter.owner == WEB
+    assert arbiter.may_drive(WEB)
+    assert now == 100.0
+
+
+def test_a_held_arm_is_not_handed_over_for_the_asking():
+    arbiter = build()
+    arbiter.take(GAMEPAD, 100.0)
+
+    assert arbiter.take(WEB, 101.0) is False
+    assert arbiter.owner == GAMEPAD
+
+
+def test_a_non_owner_may_not_drive():
+    arbiter = build()
+    arbiter.take(GAMEPAD, 100.0)
+
+    assert arbiter.may_drive(WEB) is False
+
+
+def test_preempting_takes_a_held_arm():
+    # The console offers this deliberately: an operator who can see the arm is
+    # better placed than this code to decide they should have it.
+    arbiter = build()
+    arbiter.take(GAMEPAD, 100.0)
+
+    assert arbiter.take(WEB, 101.0, preempt=True) is True
+    assert arbiter.owner == WEB
+
+
+def test_taking_an_arm_you_already_hold_is_not_a_conflict():
+    arbiter = build()
+    arbiter.take(WEB, 100.0)
+
+    assert arbiter.take(WEB, 101.0) is True
+
+
+def test_releasing_frees_the_arm():
+    arbiter = build()
+    arbiter.take(WEB, 100.0)
+
+    arbiter.release(WEB)
+
+    assert arbiter.owner is None
+
+
+def test_releasing_an_arm_somebody_else_holds_does_nothing():
+    arbiter = build()
+    arbiter.take(GAMEPAD, 100.0)
+
+    arbiter.release(WEB)
+
+    assert arbiter.owner == GAMEPAD
+
+
+def test_an_idle_gamepad_hands_the_arm_back():
+    # A pad left on the bench would otherwise lock the browser out forever.
+    arbiter = build(idle_release_s=2.0)
+    arbiter.take(GAMEPAD, 100.0)
+
+    arbiter.expire_idle_gamepad(102.0)
+
+    assert arbiter.owner is None
+
+
+def test_a_gamepad_still_being_used_keeps_the_arm():
+    arbiter = build(idle_release_s=2.0)
+    arbiter.take(GAMEPAD, 100.0)
+
+    arbiter.note_activity(GAMEPAD, 101.5)
+    arbiter.expire_idle_gamepad(102.0)
+
+    assert arbiter.owner == GAMEPAD
+
+
+def test_the_web_never_expires_on_its_own():
+    # It took the arm by pressing a button and gives it back by pressing another.
+    arbiter = build(idle_release_s=2.0)
+    arbiter.take(WEB, 100.0)
+
+    arbiter.expire_idle_gamepad(999.0)
+
+    assert arbiter.owner == WEB
+
+
+def test_activity_from_a_non_owner_does_not_extend_anyone_else():
+    arbiter = build(idle_release_s=2.0)
+    arbiter.take(GAMEPAD, 100.0)
+
+    arbiter.note_activity(WEB, 101.9)
+    arbiter.expire_idle_gamepad(102.0)
+
+    assert arbiter.owner is None