add dora-joy node publishing gamepad axes and buttons

dora_joy/__init__.py

@@ -0,0 +1 @@
+"""A gamepad, read once and published for whoever wants it."""

dora_joy/config.py

@@ -0,0 +1,40 @@
+"""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
+
+
+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 Config:
+    """The node's settings.
+
+    Short on purpose. A deadzone, a response curve and a speed all describe what
+    a reading should *do*, and this node does not know — it reads a pad and says
+    so. Adding them here would mean two places shaping the same number.
+    """
+
+    joystick_index: int
+
+    @classmethod
+    def from_env(cls) -> "Config":
+        """Build the configuration from the environment, refusing anything unusable."""
+        index = env_int("JOYSTICK_INDEX", 0)
+        if index < 0:
+            raise ValueError(f"JOYSTICK_INDEX must not be negative, not {index}")
+        return cls(joystick_index=index)

dora_joy/gamepad.py

@@ -0,0 +1,121 @@
+"""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 past this node's edge.
+
+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
+from dataclasses import dataclass, field
+
+from dora_joy.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.
+
+    Nothing here is shaped, scaled or filtered. A deadzone belongs to whoever is
+    deciding what the pad *means*, which is not this node.
+    """
+
+    axes: list[float] = field(default_factory=list)
+    buttons: list[bool] = field(default_factory=list)
+    present: bool = False
+
+
+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="dora-joy", 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. This node publishes
+        `present: false` and keeps looking, so a consumer learns that the pad
+        went away rather than that the dataflow did.
+        """
+        # 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

dora_joy/log.py

@@ -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"[joy {time.strftime('%H:%M:%S')}] {message}", file=sys.stderr, flush=True)

dora_joy/main.py

@@ -0,0 +1,69 @@
+"""The dora event loop: one reading published per tick.
+
+Every tick, changed or not. That is deliberate — the stream is also a heartbeat,
+and it is what lets a consumer tell "the pad is being held still" from "this node
+died". A consumer driving anything that moves needs both answers, and only one of
+them can be inferred from silence.
+
+Nothing here decides what a reading means. The pad is read in
+:mod:`dora_joy.gamepad` and packed in :mod:`dora_joy.message`, and that is the
+whole node.
+"""
+
+import argparse
+
+from dora import Node
+
+from dora_joy import message
+from dora_joy.config import Config
+from dora_joy.gamepad import Gamepad
+from dora_joy.log import log
+
+
+def main() -> None:
+    """Start reading the pad and publish it until the tick closes."""
+    args = _parse_args()
+    config = Config.from_env()
+
+    # Passing the name lets the node also run standalone as a dynamic node; when
+    # dora spawns it, the id from the dataflow wins.
+    node = Node(args.name)
+    gamepad = Gamepad(config.joystick_index)
+    gamepad.start()
+    log(f"publishing joystick {config.joystick_index} on every tick")
+
+    present = None
+
+    try:
+        for event in node:
+            if event["type"] == "ERROR":
+                raise RuntimeError(event["error"])
+            if event["type"] == "INPUT_CLOSED" and event.get("id") == "tick":
+                break
+            if event["type"] != "INPUT" or event["id"] != "tick":
+                continue
+
+            sample = gamepad.read()
+            # A pad arriving or going away is worth one line each; the readings
+            # themselves are on the wire for anyone who wants them.
+            if sample.present != present:
+                present = sample.present
+                log("gamepad connected" if present else "no gamepad")
+            node.send_output("joy", message.pack(sample))
+    finally:
+        gamepad.shutdown()
+
+
+def _parse_args() -> argparse.Namespace:
+    """Read the node name, for when dora did not spawn us."""
+    parser = argparse.ArgumentParser(description="Publish a gamepad's axes and buttons into a dataflow.")
+    parser.add_argument(
+        "--name",
+        default="joy",
+        help="The name of the node in the dataflow, used when running as a dynamic node.",
+    )
+    return parser.parse_args()
+
+
+if __name__ == "__main__":
+    main()

dora_joy/message.py

@@ -0,0 +1,43 @@
+"""The wire shape of a `joy` message.
+
+dora 1.0 has no standard joystick type, so this schema is the whole contract a
+consumer gets. It is **declared, never inferred**: pyarrow reads an empty
+`axes` list as `list<null>`, which would mean the port silently changed type the
+moment somebody unplugged the pad.
+
+It is not `sensor_msgs/Joy`. There is no header, because dora already knows when
+a message arrived and a second clock in the payload only invites skew questions;
+buttons are booleans rather than ROS's `int32`, because a button is down or it is
+not; and `present` is a field rather than an absence of messages, because a
+consumer stopping a robot needs to tell "the pad is centred" from "the pad is
+gone" within a tick rather than within a timeout.
+"""
+
+import pyarrow as pa
+
+from dora_joy.gamepad import Sample
+
+JOY = pa.struct(
+    [
+        # Raw, exactly as SDL reports it: no deadzone, no expo, no rescaling.
+        # Shaping is a decision about what the pad means, which belongs to
+        # whoever is driving something with it.
+        ("axes", pa.list_(pa.float32())),
+        ("buttons", pa.list_(pa.bool_())),
+        ("present", pa.bool_()),
+    ]
+)
+
+
+def pack(sample: Sample) -> pa.Array:
+    """Build one `joy` message from a reading."""
+    return pa.array(
+        [
+            {
+                "axes": [float(value) for value in sample.axes],
+                "buttons": [bool(down) for down in sample.buttons],
+                "present": sample.present,
+            }
+        ],
+        type=JOY,
+    )

dora_joy/probe.py

@@ -0,0 +1,85 @@
+"""Show what the pad is doing, with no dataflow and nothing attached to it.
+
+`dora-joy-probe` opens the gamepad and prints, live: every axis SDL reports and
+every button it sees pressed, by index.
+
+It exists because a consumer's axis and button indices are almost always SDL's
+documented layout rather than an observed one, and a pad that numbers its
+controls differently is otherwise discovered by watching a real machine move the
+wrong way. Push one control at a time and read off the index it moves.
+
+This shows the reading and nothing else — it has no idea what any index is *for*,
+which is the same thing that makes this node reusable.
+
+Nothing here can move anything. There is no dora node and no connection to
+anything; this only reads.
+"""
+
+import argparse
+import time
+
+from dora_joy.config import Config
+from dora_joy.gamepad import Gamepad
+
+CLEAR = "\033[H\033[J"
+
+
+def bar(value: float, width: int = 21) -> str:
+    """Draw a centred bar for a value between -1 and 1.
+
+    Seeing the bar sit off-centre with the stick released is how a worn pad
+    announces itself, which is most of the point of looking at raw numbers.
+    """
+    middle = width // 2
+    position = middle + round(max(-1.0, min(1.0, value)) * middle)
+    cells = ["-"] * width
+    cells[middle] = "|"
+    cells[position] = "#"
+    return "".join(cells)
+
+
+def render(sample, config: Config) -> str:
+    """Build the whole display for one reading."""
+    lines = ["dora-joy probe — Ctrl-C to stop", ""]
+
+    if not sample.present:
+        lines += [
+            "  no gamepad found.",
+            "",
+            f"  looking for joystick index {config.joystick_index};",
+            "  plug one in and this will pick it up.",
+        ]
+        return "\n".join(lines)
+
+    lines.append(f"  axes ({len(sample.axes)} reported)")
+    for index, value in enumerate(sample.axes):
+        lines.append(f"    {index}  {value:+.3f}  {bar(value)}")
+
+    held = [str(index) for index, down in enumerate(sample.buttons) if down]
+    lines += ["", f"  buttons ({len(sample.buttons)} reported)", f"    {', '.join(held) or 'none pressed'}"]
+    lines += ["", "  push one control at a time and note the index it moves."]
+    return "\n".join(lines)
+
+
+def main() -> None:
+    """Open the pad and print what it is doing until interrupted."""
+    parser = argparse.ArgumentParser(description="Show what the gamepad is doing. Moves nothing.")
+    parser.add_argument("--hz", type=float, default=10.0, help="Redraw rate. Defaults to 10.")
+    args = parser.parse_args()
+
+    config = Config.from_env()
+    gamepad = Gamepad(config.joystick_index)
+    gamepad.start()
+
+    try:
+        while True:
+            print(CLEAR + render(gamepad.read(), config), flush=True)
+            time.sleep(1.0 / args.hz)
+    except KeyboardInterrupt:
+        print()
+    finally:
+        gamepad.shutdown()
+
+
+if __name__ == "__main__":
+    main()