diff --git a/dora_teleop/arbiter.py b/dora_teleop/arbiter.py new file mode 100644 index 0000000..38196df --- /dev/null +++ b/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 diff --git a/tests/test_arbiter.py b/tests/test_arbiter.py new file mode 100644 index 0000000..db4eecd --- /dev/null +++ b/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