cristhian
04f0d57068
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.
79 lines
3.0 KiB
Python
79 lines
3.0 KiB
Python
"""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
|