dora-teleop/dora_teleop/probe.py

"""Show what the pad is doing to the arm, without an arm.

`dora-teleop-probe` attaches to a running dataflow as a dynamic node, reads the
same `joy` stream this node reads, and prints: the raw axes, the buttons, and the
velocity this node would send. **It cannot move anything.** It publishes no
output and never talks to the arm; it only reads.

It exists because the axis and button indices in :mod:`dora_teleop.mapping` are
SDL's documented layout rather than an observed one, and a pad that disagrees is
otherwise discovered by watching a real arm move the wrong way. It goes through
the same :func:`~dora_teleop.mapping.velocity` the node runs, on the same
readings, so what it prints is what the arm would get — keep it that way, or it
stops being evidence.

Because it reads `joy` rather than a pad, the dataflow has to declare it:

```yaml
  - id: teleop-probe
    path: dynamic
    inputs:
      joy: joy/joy
```

`dataflow-gamepad.yml` in this repo already does. Start that dataflow under a
coordinator (`dora start`, not `dora run`), then run `dora-teleop-probe` in
another terminal.

For a pad whose layout is not known at all — which index is which — use
`dora-joy-probe` from the `dora-joy` repo, which needs no dataflow.
"""

import argparse

from dora import Node

from dora_teleop import joy, mapping
from dora_teleop.config import Config
from dora_teleop.mapping import LAYOUT, axis_names

# What each velocity component is, in the order the arm takes them.
COMPONENTS = (
    ("vx", "mm/s", "forward / back"),
    ("vy", "mm/s", "left / right"),
    ("vz", "mm/s", "up / down"),
    ("wx", "deg/s", "roll"),
    ("wy", "deg/s", "pitch"),
    ("wz", "deg/s", "yaw"),
)

CLEAR = "\033[H\033[J"


def button_names(layout=LAYOUT) -> dict[int, str]:
    """Name each button index the layout uses."""
    return {
        layout.grasp: "grasp",
        layout.release: "release",
        layout.clear: "clear",
        layout.freedrive: "freedrive",
        layout.yaw_left: "yaw left",
        layout.yaw_right: "yaw right",
        layout.estop: "E-STOP",
    }


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, driven, velocity: list[float], config: Config, mismatch: str | None = None) -> str:
    """Build the whole display for one reading.

    ``sample`` is the pad exactly as it was reported, because seeing the raw
    numbers is the point of this tool. ``driven`` is the same reading after
    calibration — what the node actually decides from — and ``velocity`` is what
    it would send. A pad this node refuses reaches here as an absent ``driven``
    and a ``mismatch`` saying why.
    """
    lines = ["dora-teleop probe — Ctrl-C to stop", ""]

    if not sample.present:
        lines += [
            "  no gamepad.",
            "",
            "  `dora-joy` is reporting no pad, or has stopped reporting at all.",
            "  Either way the arm is being told to hold still.",
        ]
        return "\n".join(lines)

    if mismatch:
        lines += [
            "  this pad does not match the layout, and the arm will not be driven from it.",
            "",
            f"    {mismatch}",
            "",
            "  Either something was being held when it was plugged in, or the indices",
            "  below are numbered differently on this pad. The axes are raw, so they",
            "  say which: fix `Layout` in dora_teleop/mapping.py to match them.",
            "",
        ]

    names = axis_names()
    lines.append(f"  axes ({len(sample.axes)} reported)")
    for index, value in enumerate(sample.axes):
        name = names.get(index, "unused")
        lines.append(f"    {index}  {name:<15} {value:+.3f}  {bar(value)}")

    labels = button_names()
    held = [f"{index}:{labels.get(index, 'unused')}" for index, down in enumerate(sample.buttons) if down]
    lines += ["", f"  buttons ({len(sample.buttons)} reported)", f"    {', '.join(held) or 'none pressed'}"]

    lines += ["", "  velocity this would send to the arm"]
    for (name, units, meaning), value in zip(COMPONENTS, velocity, strict=True):
        marker = "  <--" if value else ""
        lines.append(f"    {name}  {value:+8.2f} {units:<6} {meaning:<14}{marker}".rstrip())

    moving = "yes" if mapping.is_moving(velocity) else "no — the arm would be told to hold still"
    lines += [
        "",
        f"  moving: {moving}",
        f"  asking for the arm: {'yes' if mapping.is_active(driven) else 'no'}",
    ]
    return "\n".join(lines)


def main() -> None:
    """Attach to a running dataflow and print what the pad is doing."""
    parser = argparse.ArgumentParser(description="Show what the gamepad would do to the arm. Moves nothing.")
    parser.add_argument(
        "--name",
        default="teleop-probe",
        help="The id this probe is declared under in the dataflow. Defaults to teleop-probe.",
    )
    args = parser.parse_args()

    config = Config.from_env()
    calibration = joy.Calibration()
    node = Node(args.name)

    for event in node:
        if event["type"] != "INPUT" or event["id"] != "joy":
            continue
        sample = joy.decode(event)
        # Through the same calibration the node drives from, so what this prints
        # stays what the arm would get. What is displayed is still the raw
        # reading: the indices are what a pad is checked against.
        driven = calibration.apply(sample)
        velocity = mapping.velocity(driven.axes, driven.buttons, config) if driven.present else [0.0] * 6
        print(CLEAR + render(sample, driven, velocity, config, calibration.mismatch), flush=True)


if __name__ == "__main__":
    main()