cristhianandClaude Opus 5
4427da9c34
_run_control never drained the job queue while the arm had not been reached, so every request that arrived during an outage waited for a connection that might never come. That breaks the one-response-per-request rule the node states as absolute, and it was circular: requests are queued before the arm is reached because recover is what a caller sends then, yet recover is served by the control thread and so was the one service that could never run while down. Serve the queue during the wait between connection attempts. Non-motion services are tried on the arm, which is what lets recover and clear reach a controller that answers while refusing to serve. A motion is refused rather than held: the arm is somewhere unknown and nobody is watching, so a held motion would fire unattended the moment the link came back. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
316 lines
11 KiB
Python
316 lines
11 KiB
Python
"""Starting up, jogging and shutting down: the node's behaviour as a dora node."""
|
|
|
|
import threading
|
|
import time
|
|
|
|
import pytest
|
|
|
|
from dora_ulite6.config import Config, MotionConfig
|
|
from dora_ulite6.control import MOVING, PREEMPTED, READY, UNAVAILABLE, Control, ServiceCall
|
|
from dora_ulite6.pose import RobotPose
|
|
from tests.conftest import FakeArm, wait_for
|
|
|
|
|
|
def build(monkeypatch, connected=True, **config):
|
|
"""Return a started Control over a fake arm, and the arms it built."""
|
|
arms = []
|
|
|
|
def make(cfg):
|
|
arm = FakeArm(cfg)
|
|
arm.connected = connected
|
|
arms.append(arm)
|
|
return arm
|
|
|
|
monkeypatch.setattr("dora_ulite6.control.ULite6", make)
|
|
restarted = config.pop("restarted", False)
|
|
control = Control(Config(telemetry_interval_s=0.01, **config), restarted=restarted)
|
|
control.arms = arms
|
|
control.start()
|
|
wait_for(lambda: control.robot is not None)
|
|
return control
|
|
|
|
|
|
# --- shutting down ---
|
|
|
|
|
|
def test_shutdown_halts_the_arm_before_leaving(monkeypatch):
|
|
# A motion lives in the controller, not in this process: exiting without
|
|
# commanding a stop leaves an arm travelling with nobody left to stop it.
|
|
control = build(monkeypatch)
|
|
control.arms[0].block = threading.Event()
|
|
control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
|
|
wait_for(lambda: "go_home" in control.arms[0].calls)
|
|
|
|
assert control.shutdown() is True
|
|
assert "stop" in control.arms[0].calls
|
|
|
|
|
|
def test_shutdown_reports_a_control_thread_it_could_not_get_back(monkeypatch):
|
|
# The caller's cue to leave rather than hang until dora kills the process.
|
|
control = build(monkeypatch, motion=MotionConfig(ready_timeout_s=0.1))
|
|
control.arms[0].block = threading.Event()
|
|
# An arm that ignores the stop is one whose thread will not come back.
|
|
control.arms[0].stop = lambda: control.arms[0].calls.append("stop")
|
|
control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
|
|
wait_for(lambda: "go_home" in control.arms[0].calls)
|
|
|
|
assert control.shutdown() is False
|
|
|
|
control.arms[0].block.set()
|
|
|
|
|
|
def test_shutdown_is_clean_when_the_arm_was_idle(monkeypatch):
|
|
control = build(monkeypatch)
|
|
|
|
assert control.shutdown() is True
|
|
|
|
|
|
# --- restarting ---
|
|
|
|
|
|
@pytest.mark.parametrize("init_mode", ["home", "pose"])
|
|
def test_a_restart_leaves_the_arm_where_it_is(monkeypatch, init_mode):
|
|
# Driving a real arm to INIT_MODE because the process came back is a motion
|
|
# nobody asked for.
|
|
control = build(monkeypatch, init_mode=init_mode, restarted=True)
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
assert control.arms[0].calls == []
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_first_start_still_runs_init_mode(monkeypatch):
|
|
control = build(monkeypatch, init_mode="home")
|
|
try:
|
|
wait_for(lambda: "go_home" in control.arms[0].calls)
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
# --- an arm that is not serving yet ---
|
|
|
|
|
|
def messages_on(control, output, seen):
|
|
"""Collect messages on one output, draining the outbox as the event loop would."""
|
|
seen.extend(control.take_messages())
|
|
return [message for message in seen if message.output == output]
|
|
|
|
|
|
def test_a_request_sent_while_the_arm_is_down_is_still_answered(monkeypatch):
|
|
# An arm that is down is an outcome to report, not a reason to stop
|
|
# answering. `recover` in particular has to reach a controller that answers
|
|
# while refusing to serve — that state is the one it exists to escape.
|
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
|
seen = []
|
|
try:
|
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
|
|
|
control.submit(ServiceCall(request_id="r1", row={"service": "recover"}))
|
|
|
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
|
assert responses[0].metadata["request_id"] == "r1"
|
|
assert responses[0].fields["ok"] is True
|
|
assert "recover" in control.arms[0].calls
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_motion_sent_while_the_arm_is_down_is_refused_rather_than_held(monkeypatch):
|
|
# Holding it would be worse than silence: the arm is somewhere unknown and
|
|
# nobody is watching, so a held motion would fire unattended the moment the
|
|
# link came back.
|
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
|
seen = []
|
|
try:
|
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
|
|
|
control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}))
|
|
|
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
|
assert responses[0].metadata["request_id"] == "m1"
|
|
assert responses[0].fields["ok"] is False
|
|
|
|
control.arms[0].connected = True
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
assert "go_home" not in control.arms[0].calls
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
# --- jogging ---
|
|
|
|
|
|
def test_a_jog_makes_the_arm_report_that_it_is_moving(monkeypatch):
|
|
# An arm that is travelling must never look idle.
|
|
control = build(monkeypatch)
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
|
|
assert MOVING in control.take_state_changes()
|
|
wait_for(lambda: any(c[0] == "jog" for c in control.arms[0].calls if isinstance(c, tuple)))
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_stream_of_jogs_is_one_state_change_not_hundreds(monkeypatch):
|
|
control = build(monkeypatch)
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
for _ in range(20):
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
|
|
assert control.take_state_changes() == [MOVING]
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch):
|
|
control = build(monkeypatch)
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
control.take_state_changes()
|
|
|
|
control.start_jog([0.0] * 6)
|
|
|
|
assert READY in control.take_state_changes()
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatch):
|
|
# A teleop source streaming zero velocity says nothing about a motion that
|
|
# owns the arm, and must not answer for it: an arm that is travelling must
|
|
# never look idle, and a `ready` here would also stop the motion's own end
|
|
# from ever being published.
|
|
control = build(monkeypatch)
|
|
control.arms[0].block = threading.Event()
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
|
|
wait_for(lambda: MOVING in control.take_state_changes())
|
|
|
|
for _ in range(5):
|
|
control.start_jog([0.0] * 6)
|
|
|
|
assert control.take_state_changes() == []
|
|
|
|
control.arms[0].block.set()
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
|
|
# The controller expires the command on its own; nothing tells us, so the
|
|
# clock has to.
|
|
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
control.take_state_changes()
|
|
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_jog_preempts_a_motion_that_is_running(monkeypatch):
|
|
control = build(monkeypatch)
|
|
control.arms[0].block = threading.Event()
|
|
try:
|
|
control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}, wait=False))
|
|
wait_for(lambda: "go_home" in control.arms[0].calls)
|
|
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
|
|
wait_for(lambda: "stop" in control.arms[0].calls)
|
|
states = _motion_states(control, "m1")
|
|
wait_for(lambda: PREEMPTED in _motion_states(control, "m1", states))
|
|
finally:
|
|
control.arms[0].block.set()
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_motion_leaves_velocity_mode_through_the_front_door(monkeypatch):
|
|
# A pose command switches the controller out of velocity mode behind the
|
|
# driver's back, leaving its own bookkeeping wrong.
|
|
control = build(monkeypatch)
|
|
try:
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
|
wait_for(lambda: any(isinstance(c, tuple) and c[0] == "jog" for c in control.arms[0].calls))
|
|
|
|
control.start_motion(
|
|
ServiceCall(request_id="m1", row={"service": "move_to", "x": 1.0, "y": 2.0, "z": 3.0}, wait=False)
|
|
)
|
|
wait_for(lambda: any(isinstance(c, tuple) and c[0] == "move_to" for c in control.arms[0].calls))
|
|
|
|
jogs = [c for c in control.arms[0].calls if isinstance(c, tuple) and c[0] == "jog"]
|
|
assert jogs[-1] == ("jog", [0.0] * 6)
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def _motion_states(control, request_id, into=None):
|
|
collected = into if into is not None else []
|
|
collected.extend(
|
|
message.fields["state"]
|
|
for message in control.take_messages()
|
|
if message.output == "motion_state" and message.fields["request_id"] == request_id
|
|
)
|
|
return collected
|
|
|
|
|
|
# --- the report invariant get_state rests on ---
|
|
|
|
|
|
def test_get_state_reads_the_cache_not_the_wire(monkeypatch):
|
|
control = build(monkeypatch)
|
|
try:
|
|
control.arms[0].pose = RobotPose(1, 2, 3)
|
|
control.call_now("s1", {"service": "get_state"})
|
|
|
|
fields = control.take_messages()[0].fields
|
|
|
|
assert fields["pose"][:3] == [1, 2, 3]
|
|
# Nothing that costs a round trip was called.
|
|
assert control.arms[0].calls == []
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
# --- the cadence itself ---
|
|
|
|
|
|
def test_the_reporter_honours_the_telemetry_interval(monkeypatch):
|
|
"""The interval is a real sleep, not a flag that makes every wait return at once.
|
|
|
|
Worth a test of its own because the failure is silent: the node behaves
|
|
correctly in every other respect while burning a core and reading the arm a
|
|
million times a second.
|
|
"""
|
|
reads = []
|
|
|
|
class Counting(FakeArm):
|
|
def read_report(self):
|
|
reads.append(1)
|
|
return super().read_report()
|
|
|
|
monkeypatch.setattr("dora_ulite6.control.ULite6", Counting)
|
|
control = Control(Config(telemetry_interval_s=0.05))
|
|
control.start()
|
|
try:
|
|
wait_for(lambda: reads)
|
|
reads.clear()
|
|
time.sleep(0.3)
|
|
finally:
|
|
control.shutdown()
|
|
|
|
# ~6 at a 50 ms cadence. A spinning loop reaches six figures.
|
|
assert len(reads) < 30, f"the reporter read the arm {len(reads)} times in 0.3 s"
|
|
assert reads, "the reporter stopped reading the arm altogether"
|