read the gamepad from dora-joy and publish twist and command
The pad moves to dora-joy, so this node keeps no hardware dependency and the pad becomes reusable. It arrives on `joy` as a plain reading; `JoyInput.read` treats silence past JOY_TTL_S as absence, because that node dying looks like silence rather than like an unplugged pad and the arm cannot tell them apart. Both control sources are now normalised to a [-1, 1] six-vector before anything is scaled, and the scaling happens in one function. The web path used to pass a velocity straight through without going near mapping, which is why the console had to speak the arm's units. It no longer does. `twist` (SI) and `command` (robot-agnostic verbs) are the standard ports. `jog` and `request` still carry the same intent in dora-ulite6's units and service names so the two repositories can move independently; LEGACY_REQUEST is the only place that knows what that node calls a verb. Wire one or the other, never both. `joint_jog` is specified in the manifest and deliberately not built. Schemas are declared rather than inferred: pyarrow reads a null `value` as its own type, so an inferred `command` would change its port's type between one verb and the next. Three bugs found while moving this code, each with a test: - `Sample.active` compared raw axes against a threshold, but SDL rests a trigger at -1.0, so every connected pad read as in use for ever: the pad took the lease on plug-in and GAMEPAD_IDLE_RELEASE_S never fired. - The e-stop latch was never consulted when deciding a velocity, so Back stopped the arm for one tick and the next stick nudge drove it again. Nothing on the pad could clear it either. It is now checked above the lease, and X releases it and clears the controller's error, as the console's RESET does. - Buttons were read before the arbiter granted the lease, so the first press after the pad had been idle was swallowed: not owned yet when read, and no longer a rising edge by the next tick.
This commit is contained in:
@@ -0,0 +1,83 @@
|
||||
"""The gamepad, as it arrives from `dora-joy`.
|
||||
|
||||
The pad itself lives in another node now. What reaches this one is a plain
|
||||
reading on the `joy` input, and this module is the seam the loop reads it
|
||||
through: :class:`Sample` is what the rest of the node sees, exactly as it did
|
||||
when the pad was read in-process.
|
||||
|
||||
**The safety-critical part is :meth:`JoyInput.read`.** Moving the pad into
|
||||
another process added a way for it to go away that did not exist before — that
|
||||
process dying — and from here that looks like silence rather than like an
|
||||
unplugged pad. So silence past `JOY_TTL_S` reads as *absent*, which is the same
|
||||
answer, because a reading nobody is refreshing is not evidence of anything.
|
||||
`dora-joy` publishes on every tick whether or not the pad moved, so silence
|
||||
means one thing only.
|
||||
"""
|
||||
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
|
||||
@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
|
||||
|
||||
|
||||
ABSENT = Sample()
|
||||
|
||||
|
||||
def decode(event: dict) -> Sample:
|
||||
"""Read one `joy` message into a reading.
|
||||
|
||||
A message this node cannot make sense of reads as absent rather than as
|
||||
centred, for the same reason an unplugged pad does.
|
||||
"""
|
||||
value = event["value"]
|
||||
if not len(value):
|
||||
return ABSENT
|
||||
row = value[0].as_py()
|
||||
return Sample(
|
||||
axes=list(row.get("axes") or []),
|
||||
buttons=[bool(down) for down in row.get("buttons") or []],
|
||||
present=bool(row.get("present")),
|
||||
)
|
||||
|
||||
|
||||
class JoyInput:
|
||||
"""The latest reading, and how long ago it arrived."""
|
||||
|
||||
def __init__(self, ttl_s: float):
|
||||
self.ttl_s = ttl_s
|
||||
self._sample = ABSENT
|
||||
self._seen_at = 0.0
|
||||
|
||||
def accept(self, sample: Sample, moment: float) -> None:
|
||||
"""Take one reading off the wire."""
|
||||
self._sample = sample
|
||||
self._seen_at = moment
|
||||
|
||||
def read(self, moment: float) -> Sample:
|
||||
"""Return the current reading, or absence if nobody is still sending one.
|
||||
|
||||
A stale reading is not a reading. `dora-joy` publishes every tick, so
|
||||
silence means it died, and a dead pad node is indistinguishable from an
|
||||
unplugged pad as far as the arm is concerned — which is the only safe
|
||||
way to read it.
|
||||
"""
|
||||
if moment - self._seen_at > self.ttl_s:
|
||||
return ABSENT
|
||||
return self._sample
|
||||
|
||||
|
||||
def now() -> float:
|
||||
"""Return the clock the deadman runs on: monotonic, so it cannot step backwards."""
|
||||
return time.monotonic()
|
||||
Reference in New Issue
Block a user