add a probe that shows what the gamepad is doing

The axis and button indices in mapping.py are SDL's documented layout rather than an observed one, and until now the only way to find a pad that numbers things differently was to watch a real arm move the wrong way. `dora-teleop-probe` prints the raw axes, the buttons it sees and the velocity that would be sent, live, with no dataflow and no arm — it cannot move anything. It goes through the same Gamepad and velocity() the node runs, so what it prints is what the arm would get; that is the whole reason it is worth having, and the reason it must not grow its own copy of the mapping. Every axis is listed, including ones the layout does not use, because an axis marked unused that moves when a stick is pushed is exactly the finding this is for. The centred bar makes a worn stick that does not return to zero obvious.
2026-09-15 15:47:36 -05:00
parent 7250396a1f
commit 8a7bfb02e0
5 changed files with 262 additions and 2 deletions
@@ -38,7 +38,7 @@ expected next to this one.
 ## Test

 ```bash
-uv run pytest      # 66 tests, no gamepad, arm, browser or daemon needed
+uv run pytest      # 76 tests, no gamepad, arm, browser or daemon needed
 uv run ruff check .
 uv run ruff format --check .
 ```
@@ -77,6 +77,12 @@ a source goes away**.
  first time somebody picks up the pad. Errors name the variable.
 - **`dora_teleop/log.py`** — Timestamped stderr logging. stderr because dora reads a
  dynamic node's stdout.
+- **`dora_teleop/probe.py`** — `dora-teleop-probe`: shows the raw axes, the buttons and the
+  velocity that would be sent, with no dataflow and no arm. It exists because the indices
+  in `mapping.py` are SDL's documented layout rather than an observed one, and a pad that
+  disagrees is otherwise found by watching a real arm move the wrong way. It goes through
+  the same `Gamepad` and `velocity()` the node runs, so what it prints is what the arm
+  would get — keep it that way, or it stops being evidence.

 ## Rules that are not negotiable

@@ -38,6 +38,46 @@ somebody can read.
 `home` is deliberately **not** on the pad. It is a large unattended motion and belongs
 behind a deliberate click, not a thumb resting on a face button.

+### Checking your pad
+
+```bash
+uv run dora-teleop-probe
+```
+
+Prints the raw axes, the buttons it sees, and the velocity this node would send — live,
+with no dataflow and no arm. **It cannot move anything.** There is no dora node and no
+connection to the robot; it only reads.
+
+```text
+  axes (6 reported)
+    0  left stick X    +0.600  ----------|-----#----
+    1  left stick Y    -0.800  --#-------|----------
+    2  left trigger    -1.000  #---------|----------
+    ...
+  buttons (6 reported)
+    5:yaw right
+
+  velocity this would send to the arm
+    vx    +38.89 mm/s   forward / back  <--
+    vy    +23.53 mm/s   left / right    <--
+    vz    +20.39 mm/s   up / down       <--
+    wz    +30.00 deg/s  yaw             <--
+
+  moving: yes
+```
+
+Run this before trusting the table above. The indices are SDL's documented layout, not
+observed ones, and a pad that numbers its axes differently is much better found here than
+by watching an arm move the wrong way. Two things to look for:
+
+- **Every axis is listed, including ones the layout does not use** — an axis marked
+  `unused` that moves when you push a stick means the layout needs changing.
+- **A bar that does not sit centred with the stick released** is a worn pad announcing
+  itself, and a reason to raise `DEADZONE`.
+
+It goes through the same `Gamepad` and `velocity()` the node runs, so what it prints is
+what the arm would be sent.
+
 ## The lease

 Two sources, one arm. Whoever holds the lease drives; the other is ignored.
@@ -143,7 +183,7 @@ gamepad if you want one — the node starts without and says so.

 ```bash
 uv sync --group dev
-uv run pytest      # 66 tests, no gamepad, arm, browser or daemon needed
+uv run pytest      # 76 tests, no gamepad, arm, browser or daemon needed
 uv run ruff check .
 uv run ruff format --check .
 ```
@@ -0,0 +1,132 @@
+"""Show what the pad is doing, with no dataflow and no arm.
+
+`dora-teleop-probe` opens the gamepad and prints, live: the raw axes SDL
+reports, the buttons it sees pressed, and the velocity this node would send.
+
+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
+:class:`~dora_teleop.gamepad.Gamepad` and :func:`~dora_teleop.mapping.velocity`
+— the same code the node runs — so what it prints is what the arm would get.
+
+Nothing here can move anything. There is no dora node and no connection to the
+arm; this only reads.
+"""
+
+import argparse
+import time
+
+from dora_teleop import mapping
+from dora_teleop.config import Config
+from dora_teleop.gamepad import Gamepad
+from dora_teleop.mapping import LAYOUT
+
+# 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 axis_names(layout=LAYOUT) -> dict[int, str]:
+    """Name each axis index the layout uses."""
+    return {
+        layout.left_x: "left stick X",
+        layout.left_y: "left stick Y",
+        layout.left_trigger: "left trigger",
+        layout.right_x: "right stick X",
+        layout.right_y: "right stick Y",
+        layout.right_trigger: "right trigger",
+    }
+
+
+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, velocity: list[float], config: Config) -> str:
+    """Build the whole display for one reading."""
+    lines = ["dora-teleop 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)
+
+    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}"]
+    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:
+            sample = gamepad.read()
+            velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
+            print(CLEAR + render(sample, velocity, config), flush=True)
+            time.sleep(1.0 / args.hz)
+    except KeyboardInterrupt:
+        print()
+    finally:
+        gamepad.shutdown()
+
+
+if __name__ == "__main__":
+    main()
@@ -23,6 +23,7 @@ dev = ["pytest >= 8.1", "ruff >= 0.9"]

 [project.scripts]
 dora-teleop = "dora_teleop.main:main"
+dora-teleop-probe = "dora_teleop.probe:main"

 [build-system]
 requires = ["hatchling"]
@@ -0,0 +1,81 @@
+"""The probe: what it draws, without a pad or a terminal."""
+
+from dora_teleop import mapping, probe
+from dora_teleop.gamepad import Sample
+from tests.conftest import stick
+
+
+def test_a_centred_stick_puts_the_marker_in_the_middle():
+    drawn = probe.bar(0.0)
+
+    assert drawn[len(drawn) // 2] == "#"
+
+
+def test_the_bar_leans_the_way_the_stick_is_pushed():
+    left = probe.bar(-1.0)
+    right = probe.bar(1.0)
+
+    assert left.index("#") < left.index("|")
+    assert right.index("#") > right.index("|")
+
+
+def test_a_reading_beyond_full_deflection_stays_on_the_bar():
+    # SDL has been known to report slightly outside -1..1 on a worn stick.
+    assert "#" in probe.bar(-4.0)
+    assert "#" in probe.bar(4.0)
+
+
+def test_no_gamepad_says_so_and_names_the_index_it_looked_for(config):
+    config.joystick_index = 2
+
+    drawn = probe.render(Sample(present=False), [0.0] * 6, config)
+
+    assert "no gamepad found" in drawn
+    assert "index 2" in drawn
+
+
+def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config):
+    # A pad whose sticks land on indices we do not use is exactly what this is
+    # for, so nothing may be hidden.
+    sample = Sample(axes=[0.0] * 8, buttons=[], present=True)
+
+    drawn = probe.render(sample, [0.0] * 6, config)
+
+    assert "unused" in drawn
+    assert "axes (8 reported)" in drawn
+
+
+def test_a_pressed_button_is_named(config):
+    sample = stick(buttons=[True])
+
+    drawn = probe.render(sample, [0.0] * 6, config)
+
+    assert "grasp" in drawn
+
+
+def test_nothing_pressed_says_so_rather_than_showing_an_empty_line(config):
+    drawn = probe.render(stick(), [0.0] * 6, config)
+
+    assert "none pressed" in drawn
+
+
+def test_the_velocity_is_shown_in_the_units_the_arm_takes(config):
+    velocity = mapping.velocity(stick(axes=[0.0, -1.0]).axes, [], config)
+
+    drawn = probe.render(stick(axes=[0.0, -1.0]), velocity, config)
+
+    assert "mm/s" in drawn
+    assert "deg/s" in drawn
+    assert f"{config.speed_mm_s:+8.2f}" in drawn
+
+
+def test_a_still_arm_is_reported_as_still(config):
+    drawn = probe.render(stick(), [0.0] * 6, config)
+
+    assert "told to hold still" in drawn
+
+
+def test_a_moving_arm_is_reported_as_moving(config):
+    drawn = probe.render(stick(axes=[0.0, -1.0]), [10.0, 0, 0, 0, 0, 0], config)
+
+    assert "moving: yes" in drawn