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:
+37
-17
@@ -2,10 +2,22 @@
|
||||
|
||||
import json
|
||||
|
||||
import pyarrow as pa
|
||||
import pytest
|
||||
|
||||
from dora_teleop.config import Config
|
||||
from dora_teleop.gamepad import Sample
|
||||
from dora_teleop.joy import Sample
|
||||
|
||||
# The shape `dora-joy` publishes. Declared here rather than inferred for the same
|
||||
# reason it is declared there: pyarrow reads an empty `axes` list as `list<null>`,
|
||||
# so a test for an absent pad would otherwise build a message no consumer sees.
|
||||
JOY = pa.struct(
|
||||
[
|
||||
("axes", pa.list_(pa.float32())),
|
||||
("buttons", pa.list_(pa.bool_())),
|
||||
("present", pa.bool_()),
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
class RecordingNode:
|
||||
@@ -26,9 +38,17 @@ class RecordingNode:
|
||||
return [(data[0].as_py(), metadata) for name, data, metadata in self.sent if name == output_id]
|
||||
|
||||
def jogs(self):
|
||||
"""Return every velocity sent to the arm, in order."""
|
||||
"""Return every velocity sent on the deprecated `jog` port, in order."""
|
||||
return [row["velocity"] for row, _ in self.outputs("jog")]
|
||||
|
||||
def twists(self):
|
||||
"""Return every Twist sent on the standard port, in order."""
|
||||
return [row for row, _ in self.outputs("twist")]
|
||||
|
||||
def actions(self):
|
||||
"""Return the verb of every standard `command`, in order."""
|
||||
return [row["action"] for row, _ in self.outputs("command")]
|
||||
|
||||
def services(self):
|
||||
"""Return the name of every service called, in order."""
|
||||
return [row["service"] for row, _ in self.outputs("request")]
|
||||
@@ -41,20 +61,6 @@ class RecordingNode:
|
||||
self.sent.clear()
|
||||
|
||||
|
||||
class FakeGamepad:
|
||||
"""Stands in for the reader thread: hands back whatever the test set."""
|
||||
|
||||
def __init__(self, sample=None):
|
||||
self.sample = sample or Sample()
|
||||
self.stopped = False
|
||||
|
||||
def read(self):
|
||||
return self.sample
|
||||
|
||||
def shutdown(self):
|
||||
self.stopped = True
|
||||
|
||||
|
||||
def stick(axes=None, buttons=None, present=True):
|
||||
"""Build one gamepad reading, padded to a full six axes."""
|
||||
values = list(axes or [])
|
||||
@@ -67,6 +73,20 @@ def stick(axes=None, buttons=None, present=True):
|
||||
return Sample(axes=values, buttons=list(buttons or []), present=present)
|
||||
|
||||
|
||||
def joy_message(sample):
|
||||
"""Build the Arrow row `dora-joy` would publish for one 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,
|
||||
)
|
||||
|
||||
|
||||
def input_event(event_id, value, metadata=None):
|
||||
"""Build the event dict dora hands a node for one input."""
|
||||
return {"type": "INPUT", "id": event_id, "value": value, "metadata": dict(metadata or {})}
|
||||
@@ -80,8 +100,8 @@ def clean_env(monkeypatch):
|
||||
"SPEED_DEG_S",
|
||||
"DEADZONE",
|
||||
"EXPO",
|
||||
"JOYSTICK_INDEX",
|
||||
"JOG_TTL_S",
|
||||
"JOY_TTL_S",
|
||||
"GAMEPAD_IDLE_RELEASE_S",
|
||||
"STATE_POLL_S",
|
||||
):
|
||||
|
||||
+14
-4
@@ -10,7 +10,6 @@ def test_the_defaults_need_no_environment():
|
||||
|
||||
assert config.speed_mm_s == 60.0
|
||||
assert config.speed_deg_s == 30.0
|
||||
assert config.joystick_index == 0
|
||||
|
||||
|
||||
def test_the_deadman_is_shorter_than_the_arms_own_expiry():
|
||||
@@ -19,6 +18,13 @@ def test_the_deadman_is_shorter_than_the_arms_own_expiry():
|
||||
assert Config.from_env().deadman.jog_ttl_s < 0.5
|
||||
|
||||
|
||||
def test_the_layers_fire_innermost_first():
|
||||
# A stale pad reading has to be noticed before the jog deadline it feeds.
|
||||
deadman = Config.from_env().deadman
|
||||
|
||||
assert deadman.joy_ttl_s < deadman.jog_ttl_s
|
||||
|
||||
|
||||
def test_a_blank_variable_falls_back_to_the_default(monkeypatch):
|
||||
monkeypatch.setenv("SPEED_MM_S", " ")
|
||||
|
||||
@@ -32,10 +38,12 @@ def test_a_speed_that_is_not_a_number_names_itself(monkeypatch):
|
||||
Config.from_env()
|
||||
|
||||
|
||||
def test_a_joystick_index_that_is_not_whole_names_itself(monkeypatch):
|
||||
monkeypatch.setenv("JOYSTICK_INDEX", "1.5")
|
||||
def test_a_joy_ttl_at_or_above_the_jog_ttl_is_refused_at_startup(monkeypatch):
|
||||
# Layers out of order means a backstop firing before the thing it backs up,
|
||||
# which is invisible until the day something actually goes wrong.
|
||||
monkeypatch.setenv("JOY_TTL_S", "0.4")
|
||||
|
||||
with pytest.raises(ValueError, match="JOYSTICK_INDEX"):
|
||||
with pytest.raises(ValueError, match="JOY_TTL_S"):
|
||||
Config.from_env()
|
||||
|
||||
|
||||
@@ -59,9 +67,11 @@ def test_an_expo_outside_the_curve_is_refused_at_startup(monkeypatch, value):
|
||||
|
||||
def test_the_timings_can_be_tuned_without_touching_the_speeds(monkeypatch):
|
||||
monkeypatch.setenv("JOG_TTL_S", "0.25")
|
||||
monkeypatch.setenv("JOY_TTL_S", "0.1")
|
||||
monkeypatch.setenv("GAMEPAD_IDLE_RELEASE_S", "5")
|
||||
|
||||
config = Config.from_env()
|
||||
|
||||
assert config.deadman.jog_ttl_s == 0.25
|
||||
assert config.deadman.joy_ttl_s == 0.1
|
||||
assert config.deadman.gamepad_idle_release_s == 5.0
|
||||
|
||||
+245
-53
@@ -8,8 +8,8 @@ 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.gamepad import Sample
|
||||
from tests.conftest import FakeGamepad, RecordingNode, input_event, stick
|
||||
from dora_teleop.joy import Sample
|
||||
from tests.conftest import RecordingNode, input_event, joy_message, stick
|
||||
|
||||
STOP = [0.0] * 6
|
||||
PUSHED = [0.0, -1.0]
|
||||
@@ -34,19 +34,32 @@ def clock(monkeypatch):
|
||||
def build():
|
||||
config = Config.from_env()
|
||||
config.expo = 0.0
|
||||
return RecordingNode(), loop.Teleop(config), FakeGamepad()
|
||||
return RecordingNode(), loop.Teleop(config)
|
||||
|
||||
|
||||
def jog_from(node, state, gamepad, count=1):
|
||||
"""Run `count` ticks and return every velocity that reached the arm."""
|
||||
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, gamepad)
|
||||
loop._tick(node, state)
|
||||
return node.jogs()
|
||||
|
||||
|
||||
def web_jog(node, state, velocity):
|
||||
"""Deliver one console jog message."""
|
||||
payload = json.dumps({"type": "jog", "velocity": velocity})
|
||||
def drive(node, state, sample, count=1):
|
||||
"""Hold one reading for `count` ticks, the way a real pad and joy node would."""
|
||||
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."""
|
||||
payload = json.dumps({"type": "jog", "axes": axes})
|
||||
loop._accept_ui_command(node, state, input_event("ui_command", pa.array([payload])))
|
||||
|
||||
|
||||
@@ -55,10 +68,9 @@ def web_jog(node, state, velocity):
|
||||
|
||||
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, gamepad = build()
|
||||
gamepad.sample = stick(axes=PUSHED)
|
||||
node, state = build()
|
||||
|
||||
jogs = jog_from(node, state, gamepad)
|
||||
jogs = drive(node, state, stick(axes=PUSHED))
|
||||
|
||||
assert state.arbiter.owner == GAMEPAD
|
||||
assert jogs[-1][0] > 0
|
||||
@@ -67,25 +79,49 @@ def test_a_pushed_stick_takes_the_arm_and_drives_it(clock):
|
||||
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, gamepad = build()
|
||||
gamepad.sample = stick(axes=PUSHED)
|
||||
jog_from(node, state, gamepad)
|
||||
node, state = build()
|
||||
drive(node, state, stick(axes=PUSHED))
|
||||
node.clear()
|
||||
|
||||
gamepad.sample = Sample(present=False)
|
||||
jogs = jog_from(node, state, gamepad)
|
||||
jogs = drive(node, state, Sample(present=False))
|
||||
|
||||
assert jogs == [STOP]
|
||||
|
||||
|
||||
def test_a_stick_returning_to_centre_stops_the_arm(clock):
|
||||
node, state, gamepad = build()
|
||||
gamepad.sample = stick(axes=PUSHED)
|
||||
jog_from(node, state, gamepad)
|
||||
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()
|
||||
|
||||
gamepad.sample = stick()
|
||||
jogs = jog_from(node, state, gamepad)
|
||||
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 len(sent) == 4
|
||||
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]
|
||||
|
||||
@@ -93,13 +129,11 @@ def test_a_stick_returning_to_centre_stops_the_arm(clock):
|
||||
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, gamepad = build()
|
||||
gamepad.sample = stick(axes=PUSHED)
|
||||
jog_from(node, state, gamepad)
|
||||
node, state = build()
|
||||
drive(node, state, stick(axes=PUSHED))
|
||||
node.clear()
|
||||
|
||||
gamepad.sample = stick()
|
||||
jogs = jog_from(node, state, gamepad, count=5)
|
||||
jogs = drive(node, state, stick(), count=5)
|
||||
|
||||
assert jogs == [STOP]
|
||||
|
||||
@@ -107,29 +141,39 @@ def test_a_stick_at_rest_sends_one_zero_not_a_stream(clock):
|
||||
def test_a_held_stick_is_resent_every_tick(clock):
|
||||
# The arm's controller expires a velocity on its own; the re-send is what
|
||||
# keeps that backstop from firing mid-motion.
|
||||
node, state, gamepad = build()
|
||||
gamepad.sample = stick(axes=PUSHED)
|
||||
node, state = build()
|
||||
|
||||
jogs = jog_from(node, state, gamepad, count=4)
|
||||
jogs = drive(node, state, stick(axes=PUSHED), count=4)
|
||||
|
||||
assert len(jogs) == 4
|
||||
assert all(jog[0] > 0 for jog in jogs)
|
||||
|
||||
|
||||
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, gamepad = build()
|
||||
node, state = build()
|
||||
state.clients = 1
|
||||
state.arbiter.take(WEB, clock.t)
|
||||
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
|
||||
jog_from(node, state, gamepad)
|
||||
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||
ticks(node, state)
|
||||
node.clear()
|
||||
|
||||
loop._accept_clients(state, input_event("clients", pa.array([{"connected": 0, "event": "close"}])))
|
||||
jogs = jog_from(node, state, gamepad)
|
||||
jogs = ticks(node, state)
|
||||
|
||||
assert jogs == [STOP]
|
||||
assert state.arbiter.owner is None
|
||||
@@ -137,41 +181,41 @@ def test_the_last_browser_leaving_stops_the_arm(clock):
|
||||
|
||||
def test_a_browser_that_goes_quiet_stops_the_arm(clock):
|
||||
# The console refreshes every 150 ms against a 400 ms deadline.
|
||||
node, state, gamepad = build()
|
||||
node, state = build()
|
||||
state.clients = 1
|
||||
state.arbiter.take(WEB, clock.t)
|
||||
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
|
||||
jog_from(node, state, gamepad)
|
||||
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||
ticks(node, state)
|
||||
node.clear()
|
||||
|
||||
clock.advance(0.5)
|
||||
jogs = jog_from(node, state, gamepad)
|
||||
jogs = ticks(node, state)
|
||||
|
||||
assert jogs == [STOP]
|
||||
|
||||
|
||||
def test_a_browser_that_keeps_refreshing_keeps_the_arm_moving(clock):
|
||||
node, state, gamepad = build()
|
||||
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, [10.0, 0, 0, 0, 0, 0])
|
||||
loop._tick(node, state, gamepad)
|
||||
web_jog(node, state, [1.0, 0, 0, 0, 0, 0])
|
||||
loop._tick(node, state)
|
||||
sent = node.jogs()
|
||||
|
||||
assert all(jog[0] == pytest.approx(10.0) for jog in sent)
|
||||
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, gamepad = build()
|
||||
node, state = build()
|
||||
state.clients = 1
|
||||
state.arbiter.take(GAMEPAD, clock.t)
|
||||
|
||||
web_jog(node, state, [10.0, 0, 0, 0, 0, 0])
|
||||
loop._tick(node, state, gamepad)
|
||||
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())
|
||||
@@ -183,7 +227,7 @@ def test_a_non_owner_jog_is_ignored(clock):
|
||||
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()
|
||||
node, state = build()
|
||||
state.clients = 1
|
||||
state.arbiter.take(GAMEPAD, clock.t)
|
||||
|
||||
@@ -194,18 +238,87 @@ def test_estop_is_never_gated_by_the_lease(clock):
|
||||
assert "stop" in node.services()
|
||||
|
||||
|
||||
def test_the_estop_button_works_without_the_pad_holding_the_arm(clock):
|
||||
node, state, gamepad = build()
|
||||
state.arbiter.take(WEB, clock.t)
|
||||
gamepad.sample = stick(buttons=[False] * 6 + [True])
|
||||
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()
|
||||
|
||||
loop._tick(node, state, gamepad)
|
||||
drive(node, state, stick(axes=PUSHED, buttons=[False] * 6 + [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=[False] * 6 + [True]))
|
||||
assert state.view.estop is True
|
||||
node.clear()
|
||||
|
||||
drive(node, state, stick(buttons=[False, False, 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=[False] * 6 + [True]))
|
||||
state.arbiter.release(GAMEPAD)
|
||||
state.arbiter.take(WEB, clock.t)
|
||||
node.clear()
|
||||
|
||||
drive(node, state, stick(buttons=[False, False, 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=[False] * 6 + [True]))
|
||||
drive(node, state, stick(buttons=[False, False, 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=[False] * 6 + [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=[False] * 6 + [True]))
|
||||
|
||||
assert "stop" in node.services()
|
||||
|
||||
|
||||
def test_a_latched_estop_is_cleared_by_reset(clock):
|
||||
node, state, _ = build()
|
||||
node, state = build()
|
||||
state.view.estop = True
|
||||
|
||||
payload = json.dumps({"type": "reset"})
|
||||
@@ -213,3 +326,82 @@ def test_a_latched_estop_is_cleared_by_reset(clock):
|
||||
|
||||
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=[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=[False, True]))
|
||||
drive(node, state, stick(buttons=[False] * 3 + [True]))
|
||||
drive(node, state, stick(buttons=[False] * 6 + [True]))
|
||||
drive(node, state, stick(buttons=[False, False, 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=[True]), count=3)
|
||||
|
||||
assert node.actions() == ["gripper_close"]
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
"""The pad as it arrives on the wire, and what silence on that wire means."""
|
||||
|
||||
import pyarrow as pa
|
||||
|
||||
from dora_teleop.joy import ABSENT, JoyInput, Sample, decode
|
||||
from tests.conftest import input_event, joy_message, stick
|
||||
|
||||
|
||||
def joy_event(sample):
|
||||
return input_event("joy", joy_message(sample))
|
||||
|
||||
|
||||
def test_a_reading_survives_the_round_trip():
|
||||
sample = decode(joy_event(stick(axes=[0.5, -0.5], buttons=[False, True])))
|
||||
|
||||
assert sample.axes[:2] == [0.5, -0.5]
|
||||
assert sample.buttons == [False, True]
|
||||
assert sample.present is True
|
||||
|
||||
|
||||
def test_an_absent_pad_decodes_as_absent_not_as_centred():
|
||||
sample = decode(joy_event(ABSENT))
|
||||
|
||||
assert sample.present is False
|
||||
assert sample.axes == []
|
||||
|
||||
|
||||
def test_a_pad_with_fewer_axes_than_the_layout_decodes_without_complaint():
|
||||
# A three-axis flight stick is a strange thing to drive an arm with, but it
|
||||
# must not take the node down; `mapping.axis` treats a missing one as centred.
|
||||
sample = decode(joy_event(Sample(axes=[0.0, 0.0, 0.0], buttons=[False], present=True)))
|
||||
|
||||
assert len(sample.axes) == 3
|
||||
|
||||
|
||||
def test_a_message_with_no_rows_reads_as_absent():
|
||||
empty = input_event("joy", pa.array([], type=joy_message(ABSENT).type))
|
||||
|
||||
assert decode(empty) is ABSENT
|
||||
|
||||
|
||||
def test_a_fresh_reading_is_handed_back():
|
||||
joy = JoyInput(ttl_s=0.2)
|
||||
sample = stick(axes=[0.5])
|
||||
joy.accept(sample, 100.0)
|
||||
|
||||
assert joy.read(100.1) is sample
|
||||
|
||||
|
||||
def test_a_reading_nobody_is_refreshing_reads_as_absent():
|
||||
# dora-joy publishes every tick, so silence means it died — and a dead pad
|
||||
# node is indistinguishable from an unplugged pad as far as the arm cares.
|
||||
joy = JoyInput(ttl_s=0.2)
|
||||
joy.accept(stick(axes=[0.5]), 100.0)
|
||||
|
||||
assert joy.read(100.5) is ABSENT
|
||||
|
||||
|
||||
def test_nothing_ever_arriving_reads_as_absent():
|
||||
# A dataflow with no `joy` wired at all, which is a browser-only console.
|
||||
assert JoyInput(ttl_s=0.2).read(100.0) is ABSENT
|
||||
@@ -108,3 +108,35 @@ def test_a_pad_reporting_fewer_axes_is_read_as_centred_not_as_an_error(config):
|
||||
|
||||
assert velocity[VX] == 0.0
|
||||
assert velocity[WZ] == 0.0
|
||||
|
||||
|
||||
# --- is the pad being used at all ---
|
||||
|
||||
|
||||
def test_a_pad_at_rest_is_not_asking_for_the_arm():
|
||||
# SDL rests a trigger at -1.0, so a raw threshold reports every connected
|
||||
# pad as in use for ever: the pad claims the arm the moment it is plugged
|
||||
# in and GAMEPAD_IDLE_RELEASE_S never fires.
|
||||
assert mapping.is_active(stick()) is False
|
||||
|
||||
|
||||
def test_an_absent_pad_is_not_asking_for_the_arm():
|
||||
assert mapping.is_active(stick(present=False)) is False
|
||||
|
||||
|
||||
def test_a_nudged_stick_is_asking_for_the_arm():
|
||||
assert mapping.is_active(stick(axes=[0.5])) is True
|
||||
|
||||
|
||||
def test_a_held_trigger_is_asking_for_the_arm():
|
||||
assert mapping.is_active(stick(axes=[0.0, 0.0, 1.0])) is True
|
||||
|
||||
|
||||
def test_a_held_button_is_asking_for_the_arm():
|
||||
assert mapping.is_active(stick(buttons=[True])) is True
|
||||
|
||||
|
||||
def test_an_axis_the_layout_does_not_use_still_counts():
|
||||
# A pad reporting its d-pad as two extra axes is asking for the arm when
|
||||
# somebody presses it, whether or not this node maps it to anything.
|
||||
assert mapping.is_active(stick(axes=[0.0, 0.0, -1.0, 0.0, 0.0, -1.0, 1.0])) is True
|
||||
|
||||
+13
-6
@@ -1,7 +1,7 @@
|
||||
"""The probe: what it draws, without a pad or a terminal."""
|
||||
|
||||
from dora_teleop import mapping, probe
|
||||
from dora_teleop.gamepad import Sample
|
||||
from dora_teleop.joy import Sample
|
||||
from tests.conftest import stick
|
||||
|
||||
|
||||
@@ -25,13 +25,13 @@ def test_a_reading_beyond_full_deflection_stays_on_the_bar():
|
||||
assert "#" in probe.bar(4.0)
|
||||
|
||||
|
||||
def test_no_gamepad_says_so_and_names_the_index_it_looked_for(config):
|
||||
config.joystick_index = 2
|
||||
|
||||
def test_no_gamepad_says_so_and_says_the_arm_is_being_held_still(config):
|
||||
# Absence covers both "no pad" and "dora-joy stopped sending", and the probe
|
||||
# must not imply the arm is waiting on something it is not.
|
||||
drawn = probe.render(Sample(present=False), [0.0] * 6, config)
|
||||
|
||||
assert "no gamepad found" in drawn
|
||||
assert "index 2" in drawn
|
||||
assert "no gamepad" in drawn
|
||||
assert "hold still" in drawn
|
||||
|
||||
|
||||
def test_every_axis_the_pad_reports_is_shown_even_the_unused_ones(config):
|
||||
@@ -79,3 +79,10 @@ 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
|
||||
|
||||
|
||||
def test_whether_the_pad_is_asking_for_the_arm_is_shown(config):
|
||||
# The probe is where a resting trigger claiming the arm would have been
|
||||
# visible, so it says what `is_active` thinks rather than leaving it implied.
|
||||
assert "asking for the arm: no" in probe.render(stick(), [0.0] * 6, config)
|
||||
assert "asking for the arm: yes" in probe.render(stick(axes=[0.0, -1.0]), [0.0] * 6, config)
|
||||
|
||||
+8
-7
@@ -7,18 +7,18 @@ from dora_teleop.arbiter import GAMEPAD, WEB
|
||||
|
||||
|
||||
def test_a_jog_carries_its_six_numbers():
|
||||
command = ui.command_of({"type": "jog", "velocity": [1, 2, 3, 4, 5, 6]})
|
||||
command = ui.command_of({"type": "jog", "axes": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]})
|
||||
|
||||
assert command.type == "jog"
|
||||
assert command.velocity == [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
|
||||
assert command.axes == [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
|
||||
|
||||
|
||||
def test_a_jog_of_the_wrong_length_is_not_a_jog():
|
||||
assert ui.command_of({"type": "jog", "velocity": [1, 2, 3]}) is None
|
||||
assert ui.command_of({"type": "jog", "axes": [1, 2, 3]}) is None
|
||||
|
||||
|
||||
def test_a_jog_carrying_something_that_is_not_a_number_is_not_a_jog():
|
||||
assert ui.command_of({"type": "jog", "velocity": [1, 2, 3, 4, 5, "fast"]}) is None
|
||||
assert ui.command_of({"type": "jog", "axes": [1, 2, 3, 4, 5, "fast"]}) is None
|
||||
|
||||
|
||||
def test_a_message_we_have_never_heard_of_is_dropped_not_refused():
|
||||
@@ -36,8 +36,8 @@ def test_taking_control_carries_whether_it_may_preempt():
|
||||
|
||||
|
||||
def test_the_gripper_button_says_which_way():
|
||||
assert ui.command_of({"type": "gripper", "vacuum": True}).enabled is True
|
||||
assert ui.command_of({"type": "gripper", "vacuum": False}).enabled is False
|
||||
assert ui.command_of({"type": "gripper", "closed": True}).enabled is True
|
||||
assert ui.command_of({"type": "gripper", "closed": False}).enabled is False
|
||||
|
||||
|
||||
def test_a_move_needs_a_position():
|
||||
@@ -109,7 +109,8 @@ def test_the_snapshot_carries_every_key_the_page_reads_by_name():
|
||||
"arm_state",
|
||||
"owner",
|
||||
"estop",
|
||||
"vacuum",
|
||||
"gripper",
|
||||
"units",
|
||||
"freedrive",
|
||||
"position",
|
||||
"joints",
|
||||
|
||||
Reference in New Issue
Block a user