add the lease that decides who drives
Two sources want one arm and the arm cannot take both. Putting the decision in one place means there is exactly one answer to "why did it not move" rather than one answer per input path. The model is ported from the monolithic repo's RobotService lease rather than replaced with last-writer-wins, because the console is already built on it: its jog pad is gated on owning the arm and it draws an owner chip. A simpler scheme would mean rewriting the page to match the node instead of the other way round. The gamepad takes the arm implicitly, on the first nudge of a stick, so nobody has to reach for a browser to use the thing in their hands — and gives it back after sitting untouched, so a pad left on the bench cannot lock the browser out forever. The web takes and releases explicitly, and may preempt: an operator who can see the arm is better placed than this code to decide they should have it.
This commit is contained in:
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user