dora-teleop/dora_teleop/joy.py

"""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.

The other thing this seam does is :class:`Calibration`: no two pads rest in
quite the same place, and one that rests somewhere the layout does not expect
is a pad this node must not drive an arm from. Every reading the loop sees has
been through it, so the rest of the node only ever handles a pad that behaves.
"""

import time
from dataclasses import dataclass, field

from dora_teleop import mapping
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


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 Calibration:
    """Where the pad in front of us rests, and whether the layout fits it.

    Captured from the first reading of a pad that is sitting still, and kept
    until that pad goes away. It is not re-taken while the pad is in use, which
    would be the one way to get this badly wrong: an operator holding a stick
    when it happened would have that deflection recorded as centre, and the arm
    would then move whenever they let go.

    A pad whose resting axes do not fit the layout is reported **absent** rather
    than corrected. The numbering cannot be guessed from one reading — a trigger
    resting at -1.0 and a stick held hard over look identical — and guessing
    wrong drives a real arm. `dora-teleop-probe` shows what the pad reports, and
    `Layout` in :mod:`dora_teleop.mapping` is where a pad that disagrees is
    written down.
    """

    def __init__(self, layout: mapping.Layout = mapping.LAYOUT):
        self.layout = layout
        self.mismatch: str | None = None
        self._zeros: list[float] | None = None

    @property
    def known(self) -> bool:
        """Whether a pad has been measured at rest and may be driven from."""
        return self._zeros is not None

    def forget(self) -> None:
        """Drop what was learned, so the next pad to appear is measured afresh."""
        self._zeros = None
        self.mismatch = None

    def apply(self, sample: Sample) -> Sample:
        """Return the reading as the layout's ideal pad would have sent it.

        An absent pad passes straight through and takes the calibration with
        it. A pad that does not fit the layout comes back absent, which every
        path in this node already treats as a reason to stop.
        """
        if not sample.present:
            self.forget()
            return sample
        if self._zeros is None or len(self._zeros) != len(sample.axes):
            try:
                self._zeros = mapping.rest_zeros(sample.axes, self.layout)
            except ValueError as wrong:
                # Logged once per pad: this runs on every reading, and a pad
                # that does not fit the layout does not fit it fifty times a
                # second.
                if self.mismatch is None:
                    log(
                        f"ignoring the gamepad: {wrong}. Run dora-teleop-probe and check Layout in mapping.py"
                    )
                self.mismatch = str(wrong)
                return ABSENT
            self.mismatch = None
        return Sample(
            axes=mapping.canonical(sample.axes, self._zeros, self.layout),
            buttons=sample.buttons,
            present=True,
        )


class JoyInput:
    """The latest reading, and how long ago it arrived."""

    def __init__(self, ttl_s: float, layout: mapping.Layout = mapping.LAYOUT):
        self.ttl_s = ttl_s
        self.calibration = Calibration(layout)
        self._sample = ABSENT
        self._seen_at = 0.0

    def accept(self, sample: Sample, moment: float) -> None:
        """Take one reading off the wire, calibrated to the pad that sent it."""
        self._sample = self.calibration.apply(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()