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:
+48
-29
@@ -1,24 +1,40 @@
|
||||
"""Show what the pad is doing, with no dataflow and no arm.
|
||||
"""Show what the pad is doing to the arm, without an 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.
|
||||
`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
|
||||
: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.
|
||||
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.
|
||||
|
||||
Nothing here can move anything. There is no dora node and no connection to the
|
||||
arm; this only reads.
|
||||
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
|
||||
import time
|
||||
|
||||
from dora_teleop import mapping
|
||||
from dora import Node
|
||||
|
||||
from dora_teleop import joy, 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.
|
||||
@@ -79,10 +95,10 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
||||
|
||||
if not sample.present:
|
||||
lines += [
|
||||
" no gamepad found.",
|
||||
" no gamepad.",
|
||||
"",
|
||||
f" looking for joystick index {config.joystick_index};",
|
||||
" plug one in and this will pick it up.",
|
||||
" `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)
|
||||
|
||||
@@ -102,30 +118,33 @@ def render(sample, velocity: list[float], config: Config) -> str:
|
||||
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}"]
|
||||
lines += [
|
||||
"",
|
||||
f" moving: {moving}",
|
||||
f" asking for the arm: {'yes' if mapping.is_active(sample) else 'no'}",
|
||||
]
|
||||
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.")
|
||||
"""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()
|
||||
gamepad = Gamepad(config.joystick_index)
|
||||
gamepad.start()
|
||||
node = Node(args.name)
|
||||
|
||||
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()
|
||||
for event in node:
|
||||
if event["type"] != "INPUT" or event["id"] != "joy":
|
||||
continue
|
||||
sample = joy.decode(event)
|
||||
velocity = mapping.velocity(sample.axes, sample.buttons, config) if sample.present else [0.0] * 6
|
||||
print(CLEAR + render(sample, velocity, config), flush=True)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
Reference in New Issue
Block a user