add the gamepad reader and the node's settings
The reader owns the pad on a thread of its own, and nothing outside that file may touch pygame: everything else sees a plain immutable Sample. That is what lets the rest of the node be tested with no hardware, and what keeps SDL's quirks out of the arm logic. Sample.present is the field that matters. A pad that has been unplugged reads as absent rather than as centred, so "the operator let go" and "the pad fell off the desk" stay distinguishable — they call for the same stop, but only one of them is worth logging. Only the display and joystick subsystems are initialised. pygame.init() also opens audio, which on a headless robot fills stderr with ALSA errors for a device this node never uses; the display is needed despite there being no window, because SDL refreshes joystick state during the event pump. Settings are parsed once at startup so a deadzone that would silently make the pad inert stops the node before it joins the dataflow.
This commit is contained in:
@@ -0,0 +1 @@
|
||||
"""Human control of a robot arm, from a gamepad or a browser."""
|
||||
@@ -0,0 +1,99 @@
|
||||
"""Everything the node reads from the environment, parsed once at startup.
|
||||
|
||||
Parsing happens here rather than where each value is used, so an unusable
|
||||
setting stops the node before it joins the dataflow instead of surfacing the
|
||||
first time somebody picks up the gamepad.
|
||||
"""
|
||||
|
||||
import os
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
|
||||
def env_float(name: str, default: float) -> float:
|
||||
"""Read a number, naming the variable if it cannot be read as one."""
|
||||
value = os.environ.get(name, "").strip()
|
||||
if not value:
|
||||
return default
|
||||
try:
|
||||
return float(value)
|
||||
except ValueError:
|
||||
raise ValueError(f"{name} must be a number, not {value!r}") from None
|
||||
|
||||
|
||||
def env_int(name: str, default: int) -> int:
|
||||
"""Read a whole number, naming the variable if it cannot be read as one."""
|
||||
value = os.environ.get(name, "").strip()
|
||||
if not value:
|
||||
return default
|
||||
try:
|
||||
return int(value)
|
||||
except ValueError:
|
||||
raise ValueError(f"{name} must be a whole number, not {value!r}") from None
|
||||
|
||||
|
||||
@dataclass
|
||||
class DeadmanConfig:
|
||||
"""How long a source may go quiet before the arm is told to stop.
|
||||
|
||||
Three layers, outermost last. This node re-sends the current velocity every
|
||||
tick; `jog_ttl_s` is how long it will keep doing so without hearing from the
|
||||
source; and behind both sits the arm node's own `VELOCITY_COMMAND_TTL_S`,
|
||||
which the controller enforces whether or not this process is alive.
|
||||
|
||||
Keep them in that order — ours shorter than the controller's — or the
|
||||
backstop fires before the thing it is backing up.
|
||||
"""
|
||||
|
||||
jog_ttl_s: float = 0.4
|
||||
# How long a gamepad may sit untouched before it gives the arm back, so the
|
||||
# browser can take over without anyone having to press anything.
|
||||
gamepad_idle_release_s: float = 2.0
|
||||
# How often to ask the arm for the things telemetry does not carry: whether
|
||||
# the tool is engaged, whether freedrive is on, whether it is ready.
|
||||
state_poll_s: float = 0.5
|
||||
|
||||
@classmethod
|
||||
def from_env(cls) -> "DeadmanConfig":
|
||||
"""Build the timings from the environment."""
|
||||
return cls(
|
||||
jog_ttl_s=env_float("JOG_TTL_S", 0.4),
|
||||
gamepad_idle_release_s=env_float("GAMEPAD_IDLE_RELEASE_S", 2.0),
|
||||
state_poll_s=env_float("STATE_POLL_S", 0.5),
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Config:
|
||||
"""The node's settings."""
|
||||
|
||||
# Full-deflection speeds. A stick is analog, so these are the maximum and
|
||||
# anything less moves proportionally slower — which is the whole reason a
|
||||
# gamepad beats the console's dial pad.
|
||||
speed_mm_s: float
|
||||
speed_deg_s: float
|
||||
# Stick travel to ignore. A worn stick that does not centre would otherwise
|
||||
# drive the arm across the table on its own.
|
||||
deadzone: float
|
||||
# How much of the curve is cubic: 0 is linear, 1 is fully cubic. Fine
|
||||
# positioning happens near centre, and expo is what buys resolution there.
|
||||
expo: float
|
||||
joystick_index: int
|
||||
deadman: DeadmanConfig = field(default_factory=DeadmanConfig)
|
||||
|
||||
@classmethod
|
||||
def from_env(cls) -> "Config":
|
||||
"""Build the configuration from the environment, refusing anything unusable."""
|
||||
deadzone = env_float("DEADZONE", 0.12)
|
||||
if not 0.0 <= deadzone < 1.0:
|
||||
raise ValueError(f"DEADZONE must be at least 0 and below 1, not {deadzone}")
|
||||
expo = env_float("EXPO", 0.4)
|
||||
if not 0.0 <= expo <= 1.0:
|
||||
raise ValueError(f"EXPO must be between 0 and 1, not {expo}")
|
||||
return cls(
|
||||
speed_mm_s=env_float("SPEED_MM_S", 60.0),
|
||||
speed_deg_s=env_float("SPEED_DEG_S", 30.0),
|
||||
deadzone=deadzone,
|
||||
expo=expo,
|
||||
joystick_index=env_int("JOYSTICK_INDEX", 0),
|
||||
deadman=DeadmanConfig.from_env(),
|
||||
)
|
||||
@@ -0,0 +1,135 @@
|
||||
"""The gamepad, and the thread that reads it.
|
||||
|
||||
**Nothing outside this file may touch pygame.** The rest of the node sees only
|
||||
:class:`Sample`, which is a plain immutable reading — that is what lets every
|
||||
other module be tested without a pad plugged in, and what keeps SDL's quirks
|
||||
from leaking into the arm logic.
|
||||
|
||||
The reader is a thread because SDL wants to be pumped at its own cadence and
|
||||
because a pad being unplugged should be noticed within milliseconds rather than
|
||||
on whenever the dataflow next ticks. It never publishes anything; it fills a
|
||||
slot, and the event loop drains it.
|
||||
"""
|
||||
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from dora_teleop.log import log
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Sample:
|
||||
"""One reading of the pad.
|
||||
|
||||
``present`` is the one that matters for safety: a pad that has been
|
||||
unplugged reads as absent rather than as centred, so the difference between
|
||||
"the operator let go" and "the operator's pad fell off the desk" is not lost.
|
||||
"""
|
||||
|
||||
axes: list[float] = field(default_factory=list)
|
||||
buttons: list[bool] = field(default_factory=list)
|
||||
present: bool = False
|
||||
|
||||
@property
|
||||
def active(self) -> bool:
|
||||
"""Whether anything is being touched at all.
|
||||
|
||||
Deliberately raw — no deadzone. This decides whether the pad is asking
|
||||
for the arm, and :mod:`dora_teleop.mapping` decides how far to move it.
|
||||
A separate, tighter threshold here would mean a stick could claim the
|
||||
arm and then not move it.
|
||||
"""
|
||||
return self.present and (any(abs(a) > 0.3 for a in self.axes) or any(self.buttons))
|
||||
|
||||
|
||||
ABSENT = Sample()
|
||||
|
||||
|
||||
class Gamepad:
|
||||
"""Reads one pad on a thread and keeps the latest reading available."""
|
||||
|
||||
def __init__(self, index: int, poll_interval_s: float = 0.01):
|
||||
self.index = index
|
||||
self.poll_interval_s = poll_interval_s
|
||||
self._lock = threading.Lock()
|
||||
self._sample = ABSENT
|
||||
self._stopping = threading.Event()
|
||||
self._thread = threading.Thread(target=self._run, name="teleop-gamepad", daemon=True)
|
||||
|
||||
def start(self) -> None:
|
||||
"""Start reading. Never blocks, and never fails for want of a pad."""
|
||||
self._thread.start()
|
||||
|
||||
def read(self) -> Sample:
|
||||
"""Return the most recent reading."""
|
||||
with self._lock:
|
||||
return self._sample
|
||||
|
||||
def shutdown(self) -> None:
|
||||
"""Stop reading and let the thread finish."""
|
||||
self._stopping.set()
|
||||
self._thread.join(timeout=1.0)
|
||||
|
||||
def _run(self) -> None:
|
||||
"""Open the pad, read it, and keep trying if it is not there.
|
||||
|
||||
A missing pad is an ordinary state, not an error: the browser is a whole
|
||||
control path of its own, and the node has no business refusing to start
|
||||
because nobody has plugged a controller in yet.
|
||||
"""
|
||||
# Two subsystems, not all of them. `pygame.init()` also opens audio,
|
||||
# which on a headless robot means a screenful of ALSA errors in the
|
||||
# dataflow's stderr for a device this node never uses. The display is
|
||||
# needed despite there being no window: SDL refreshes joystick state
|
||||
# during the event pump, and the pump needs the video subsystem.
|
||||
os.environ.setdefault("SDL_VIDEODRIVER", "dummy")
|
||||
os.environ.setdefault("SDL_AUDIODRIVER", "dummy")
|
||||
import pygame
|
||||
|
||||
pygame.display.init()
|
||||
pygame.joystick.init()
|
||||
joystick = None
|
||||
|
||||
while not self._stopping.is_set():
|
||||
pygame.event.pump()
|
||||
if joystick is None:
|
||||
joystick = self._open(pygame)
|
||||
if joystick is None:
|
||||
self._store(ABSENT)
|
||||
self._stopping.wait(1.0)
|
||||
continue
|
||||
try:
|
||||
sample = Sample(
|
||||
axes=[joystick.get_axis(i) for i in range(joystick.get_numaxes())],
|
||||
buttons=[bool(joystick.get_button(i)) for i in range(joystick.get_numbuttons())],
|
||||
present=True,
|
||||
)
|
||||
except Exception as error: # noqa: BLE001 — an unplugged pad raises from inside SDL
|
||||
log(f"the gamepad went away ({error})")
|
||||
joystick = None
|
||||
self._store(ABSENT)
|
||||
continue
|
||||
self._store(sample)
|
||||
self._stopping.wait(self.poll_interval_s)
|
||||
|
||||
def _open(self, pygame):
|
||||
"""Open the configured pad, or return None if it is not there."""
|
||||
pygame.joystick.quit()
|
||||
pygame.joystick.init()
|
||||
if pygame.joystick.get_count() <= self.index:
|
||||
return None
|
||||
joystick = pygame.joystick.Joystick(self.index)
|
||||
joystick.init()
|
||||
log(f"gamepad {self.index}: {joystick.get_name()}")
|
||||
return joystick
|
||||
|
||||
def _store(self, sample: Sample) -> None:
|
||||
with self._lock:
|
||||
self._sample = sample
|
||||
|
||||
|
||||
def now() -> float:
|
||||
"""Return the clock the deadman runs on: monotonic, so it cannot step backwards."""
|
||||
return time.monotonic()
|
||||
@@ -0,0 +1,13 @@
|
||||
"""Timestamped stderr logging.
|
||||
|
||||
stderr rather than stdout: dora reads a dynamic node's stdout, and a print that
|
||||
lands there is noise in the wrong stream.
|
||||
"""
|
||||
|
||||
import sys
|
||||
import time
|
||||
|
||||
|
||||
def log(message: str) -> None:
|
||||
"""Print a timestamped line for the node."""
|
||||
print(f"[teleop {time.strftime('%H:%M:%S')}] {message}", file=sys.stderr, flush=True)
|
||||
Reference in New Issue
Block a user