liria/dora-teleop
4
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5ce83ae9455be80d4d77c4cde44d9c0425870246
dora-teleop/tests/test_deadman.py
T
cristhianandClaude Opus 5 d0f884c7e2 fix gamepad layout and refuse a pad whose axes disagree 
An Xbox One S pad over Bluetooth numbers its controls differently from the
indices this node shipped with: the right stick is on axes 2 and 3, the
triggers on 4 and 5, and the buttons carry gaps at 2 and 5. Against the old
layout an untouched pad reported a resting trigger as full right-stick
deflection, which asked for 30 deg/s of pitch while lying on the desk, held
the lease for ever so GAMEPAD_IDLE_RELEASE_S never fired, and moved a real
arm into a controller error. X toggled freedrive and LB was the E-STOP, so
the resulting latch could not be released from the pad at all.

Layout now carries the swept indices. Each pad is also measured the first
time it is seen at rest: a stick must rest near centre and a trigger near
the bottom of its travel, and a pad that does not fit reads as absent and is
named in the log rather than driving an arm from a numbering nobody checked.
The measured rest becomes each axis's zero, so a worn stick's tilt is
subtracted rather than deadzoned away and a trigger that reads 0 until it
first moves stops reading as half pressed.

Rest is never re-measured while the pad is in use, because doing that under
a held stick would record the deflection as centre and the arm would move
when the operator let go. The probe drives from the same calibration, so
what it prints is still what the arm would get, and it shows the mismatch.

Tests name their controls through axes_for and buttons_for instead of
numbering them; the positional lists are why a wrong index passed a green
suite.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
2026-09-16 18:06:06 -03:00

439 lines
13 KiB
Python
Raw Blame History

"""The deadman: every way a source can go away, and the stop that follows it."""
import pytest
from dora_teleop import main as loop
from dora_teleop.arbiter import GAMEPAD, WEB
from dora_teleop.config import Config
from dora_teleop.joy import Sample
from tests.conftest import RecordingNode, buttons_for, input_event, joy_message, stick
STOP = [0.0] * 6
PUSHED = [0.0, -1.0]
@pytest.fixture
def clock(monkeypatch):
"""Return a clock the test moves by hand, so no test waits out a real timeout."""
class Clock:
def __init__(self):
self.t = 1000.0
def advance(self, seconds):
self.t += seconds
moved = Clock()
monkeypatch.setattr("dora_teleop.main.now", lambda: moved.t)
return moved
def build():
config = Config.from_env()
config.expo = 0.0
return RecordingNode(), loop.Teleop(config)
def send_joy(node, state, sample):
"""Deliver one reading the way `dora-joy` publishes it, decoding included."""
loop._handle(node, state, input_event("joy", joy_message(sample)))
def ticks(node, state, count=1):
"""Run `count` ticks with nothing new arriving, and return every velocity sent."""
for _ in range(count):
loop._tick(node, state)
return node.jogs()
def drive(node, state, sample, count=1):
"""Hold one reading for `count` ticks, the way a real pad and joy node would.
A pad this node has not yet seen at rest is sent one resting reading first,
because a real one is always at rest before a thumb reaches it — and the
node refuses to drive an arm from a pad it has never seen still.
"""
if not state.joy.calibration.known:
send_joy(node, state, stick())
for _ in range(count):
send_joy(node, state, sample)
loop._tick(node, state)
return node.jogs()
def web_jog(node, state, axes):
"""Deliver one console jog message, in the normalised units the page speaks."""
loop._accept_ui_command(node, state, {"type": "jog", "axes": axes})
# --- the gamepad ---
def test_a_pushed_stick_takes_the_arm_and_drives_it(clock):
# Nobody should have to reach for a browser to use the pad in their hands.
node, state = build()
jogs = drive(node, state, stick(axes=PUSHED))
assert state.arbiter.owner == GAMEPAD
assert jogs[-1][0] > 0
def test_a_gamepad_that_is_unplugged_mid_jog_stops_the_arm(clock):
# An unplugged pad is not a centred pad, and waiting out the TTL here is the
# difference between a dropped controller costing 10 ms of travel and 400.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
jogs = drive(node, state, Sample(present=False))
assert jogs == [STOP]
def test_the_joy_node_dying_stops_the_arm(clock):
# The pad is another process now, and that process dying looks like silence
# rather than like an unplugged pad. It has to mean the same thing: a
# reading nobody is refreshing is not evidence that the stick is still held.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
clock.advance(state.config.deadman.joy_ttl_s + 0.01)
jogs = ticks(node, state)
assert jogs == [STOP]
def test_a_joy_node_that_keeps_publishing_keeps_the_arm_moving(clock):
# The mirror of the test above: time passing is not by itself a reason to
# stop, or a held stick would stutter.
node, state = build()
sent = []
for _ in range(4):
clock.advance(0.05)
sent = drive(node, state, stick(axes=PUSHED))
assert sent
assert all(jog[0] > 0 for jog in sent)
def test_a_stick_returning_to_centre_stops_the_arm(clock):
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
jogs = drive(node, state, stick())
assert jogs == [STOP]
def test_a_stick_at_rest_sends_one_zero_not_a_stream(clock):
# A stream of zeroes would keep the arm node believing a jog is live and
# hold robot_state at `moving` forever.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
jogs = drive(node, state, stick(), count=5)
assert jogs == [STOP]
def test_a_held_stick_is_not_resent_every_tick(clock):
# A velocity is a set-point the arm holds, not a frame in a sequence.
# Repeating it every tick made the controller re-plan the same motion twenty
# times a second, which an operator feels as the arm stepping rather than
# travelling.
node, state = build()
jogs = drive(node, state, stick(axes=PUSHED), count=4)
assert len(jogs) == 1
assert jogs[0][0] > 0
def test_a_held_stick_is_repeated_before_the_arm_would_expire_it(clock):
# The repeat exists for one reason: the arm's controller drops a velocity on
# its own, and the repeat is what keeps that backstop from firing mid-motion.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
clock.advance(state.config.deadman.jog_keepalive_s)
jogs = drive(node, state, stick(axes=PUSHED))
assert len(jogs) == 1
assert jogs[0][0] > 0
def test_a_change_of_direction_never_waits_for_the_repeat(clock):
# Sending only on change is worth nothing if the change itself is delayed:
# a press or a release has to be acted on the tick it happens, not up to a
# keepalive later.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
jogs = drive(node, state, stick(axes=[0.0, 1.0]))
assert len(jogs) == 1
assert jogs[0][0] < 0
def test_a_pad_that_was_never_wired_up_leaves_the_arm_alone(clock):
# `joy` is optional: a browser-only console wires no pad at all, and the
# node must not read that as a centred stick asking for the arm.
node, state = build()
jogs = ticks(node, state, count=3)
assert jogs == []
assert state.arbiter.owner is None
# --- the browser ---
def test_the_last_browser_leaving_stops_the_arm(clock):
# Closing the tab has to stop the arm, and nothing is sent when it does.
node, state = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
ticks(node, state)
node.clear()
loop._set_clients(state, 0)
jogs = ticks(node, state)
assert jogs == [STOP]
assert state.arbiter.owner is None
def test_a_browser_that_goes_quiet_stops_the_arm(clock):
# The console refreshes every 150 ms against a 400 ms deadline.
node, state = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
ticks(node, state)
node.clear()
clock.advance(0.5)
jogs = ticks(node, state)
assert jogs == [STOP]
def test_a_browser_that_keeps_refreshing_keeps_the_arm_moving(clock):
node, state = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
sent = []
for _ in range(4):
clock.advance(0.15)
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
loop._tick(node, state)
sent = node.jogs()
assert all(jog[0] == pytest.approx(state.config.speed_mm_s) for jog in sent)
def test_a_non_owner_jog_is_ignored(clock):
node, state = build()
state.clients = 1
state.arbiter.take(GAMEPAD, clock.t)
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
loop._tick(node, state)
assert node.jogs() == []
assert any(message.get("type") == "error" for message in node.ui())
# --- the stop that is never gated ---
def test_estop_is_never_gated_by_the_lease(clock):
# Somebody reaching for the stop is the one moment when asking who owns the
# arm is the wrong question.
node, state = build()
state.clients = 1
state.arbiter.take(GAMEPAD, clock.t)
loop._accept_ui_command(node, state, {"type": "estop"})
assert node.jogs() == [STOP]
assert "stop" in node.services()
def test_a_latched_estop_keeps_the_arm_still_under_a_pushed_stick(clock):
# A stop somebody had to reach for is not undone by the next thumb
# movement. Without this the pad stops the arm for one tick and drives it
# again on the next, which is not a stop at all.
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
drive(node, state, stick(axes=PUSHED, buttons=buttons_for(estop=True)))
node.clear()
jogs = drive(node, state, stick(axes=PUSHED), count=4)
assert jogs == []
def test_the_pad_can_release_the_latch_it_set(clock):
# A pad-only dataflow has no console RESET, so a latch nothing on the pad
# can release would strand the arm until the process was restarted.
node, state = build()
drive(node, state, stick(buttons=buttons_for(estop=True)))
assert state.view.estop is True
node.clear()
drive(node, state, stick(buttons=buttons_for(clear=True)))
assert state.view.estop is False
assert "clear" in node.services()
def test_the_latch_is_released_without_holding_the_arm(clock):
# An idle pad hands the lease back two seconds after the stop it just made,
# so gating the release on the lease would be a trap.
node, state = build()
drive(node, state, stick(buttons=buttons_for(estop=True)))
state.arbiter.release(GAMEPAD)
state.arbiter.take(WEB, clock.t)
node.clear()
drive(node, state, stick(buttons=buttons_for(clear=True)))
assert state.view.estop is False
def test_the_arm_drives_again_once_the_latch_is_released(clock):
node, state = build()
drive(node, state, stick(buttons=buttons_for(estop=True)))
drive(node, state, stick(buttons=buttons_for(clear=True)))
node.clear()
jogs = drive(node, state, stick(axes=PUSHED))
assert jogs[-1][0] > 0
def test_a_latched_estop_stops_the_browser_too(clock):
# The latch is the node's, not the pad's. A console jog must not walk
# through a stop somebody made on the gamepad.
node, state = build()
state.clients = 1
drive(node, state, stick(buttons=buttons_for(estop=True)))
state.arbiter.release(GAMEPAD)
state.arbiter.take(WEB, clock.t)
node.clear()
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
jogs = ticks(node, state)
assert jogs == []
def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
node, state = build()
state.arbiter.take(WEB, clock.t)
drive(node, state, stick(buttons=buttons_for(estop=True)))
assert "stop" in node.services()
def test_a_latched_estop_is_cleared_by_reset(clock):
node, state = build()
state.view.estop = True
loop._accept_ui_command(node, state, {"type": "reset"})
assert state.view.estop is False
assert "clear" in node.services()
# --- the standard ports ---
def test_the_same_deflection_from_either_source_asks_for_the_same_motion(clock):
# The whole reason both sources are normalised before they are scaled. If
# these ever differ, one path has grown a conversion the other has not —
# which is how a console ends up in the robot's units.
node, state = build()
drive(node, state, stick(axes=[0.0, -1.0]))
from_pad = node.twists()[-1]
node, state = build()
state.clients = 1
state.arbiter.take(WEB, clock.t)
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
ticks(node, state)
from_web = node.twists()[-1]
assert from_pad == from_web
def test_the_twist_is_si_and_the_legacy_jog_is_not(clock):
# 60 mm/s is 0.06 m/s. A consumer reading the wrong port at the wrong scale
# is a thousandfold error, so this is worth pinning.
node, state = build()
drive(node, state, stick(axes=[0.0, -1.0]))
twist, jog = node.twists()[-1], node.jogs()[-1]
assert twist["linear"][0] == pytest.approx(state.config.speed_mm_s / 1000.0)
assert jog[0] == pytest.approx(state.config.speed_mm_s)
assert twist["frame"] == "base"
def test_a_stop_reaches_both_ports(clock):
node, state = build()
drive(node, state, stick(axes=PUSHED))
node.clear()
drive(node, state, stick())
assert node.jogs() == [STOP]
assert node.twists() == [{"linear": [0.0] * 3, "angular": [0.0] * 3, "frame": "base"}]
def test_a_button_asks_in_verbs_as_well_as_in_services(clock):
# The verb is what a robot speaking the standard reads; the service name is
# what dora-ulite6 reads until it does.
node, state = build()
drive(node, state, stick(buttons=buttons_for(grasp=True)))
assert node.actions() == ["gripper_close"]
assert "grasp" in node.services()
def test_every_verb_the_pad_can_ask_for_is_robot_agnostic(clock):
node, state = build()
drive(node, state, stick(buttons=buttons_for(release=True)))
drive(node, state, stick(buttons=buttons_for(freedrive=True)))
drive(node, state, stick(buttons=buttons_for(estop=True)))
drive(node, state, stick(buttons=buttons_for(clear=True)))
assert node.actions() == ["gripper_open", "freedrive", "stop", "clear"]
def test_the_first_press_after_the_pad_has_been_idle_is_not_dropped(clock):
# The lease has to be settled before the buttons are read. Read them first
# and the press arrives while nobody owns the arm, and by the next tick the
# button is held rather than newly down — so the edge is gone for good and
# the operator presses A and watches nothing happen.
node, state = build()
assert state.arbiter.owner is None
drive(node, state, stick(buttons=buttons_for(grasp=True)), count=3)
assert node.actions() == ["gripper_close"]
Reference in New Issue View Git Blame Copy Permalink