diff --git a/AGENTS.md b/AGENTS.md index 5b943a9..ad10abd 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -85,7 +85,12 @@ while it runs. So: dataflow, and so the arm can be told whether this is a restart. - **`dora_ulite6/control.py`** — `Control`: both arm threads and the mailboxes the loop drains. The **control thread** runs one job at a time and is allowed to block: startup, - the `INIT_MODE` move, and every service including the motions that take seconds. The + the `INIT_MODE` move, and every service including the motions that take seconds. Its + wait between connection attempts serves the queue (`_serve_while_down`) rather than + sleeping through it: a request is answered exactly once whatever the arm is doing, and + an arm that is down is an outcome to report, not a reason to go quiet. A motion is + refused there rather than held, since a held one would fire unattended the moment the + link came back. The **reporter thread** never blocks: it reads the report cache at the telemetry cadence, so `tcp_pose` keeps flowing right through a move. **Nothing outside this file may call the driver**, except `stop` and `read_report`, both written for another thread. Same diff --git a/dora_ulite6/control.py b/dora_ulite6/control.py index 8cc93c5..f53dc7f 100644 --- a/dora_ulite6/control.py +++ b/dora_ulite6/control.py @@ -339,11 +339,49 @@ class Control: except RobotUnavailableError as error: log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s") self._set_state(UNAVAILABLE) - self._stopping.wait(self.config.reconnect_interval_s) + self._serve_while_down(self.config.reconnect_interval_s) return self._started = True self._set_state(READY) + def _serve_while_down(self, seconds: float) -> None: + """Answer what can be answered without a serving arm, until it is time to retry. + + This is the wait between connection attempts, and it serves the queue + rather than sleeping through it. Sitting silent is the one thing a caller + must never get: an arm that is down is an outcome to report, not a reason + to stop answering, and a request left in the queue here would be answered + only if the arm ever came back. + """ + deadline = time.monotonic() + seconds + while not self._stopping.is_set(): + remaining = deadline - time.monotonic() + if remaining <= 0: + return + try: + job = self._jobs.get(timeout=min(remaining, 0.1)) + except queue.Empty: + continue + if isinstance(job, ServiceCall): + self._serve_without_arm(job) + + def _serve_without_arm(self, call: ServiceCall) -> None: + """Answer one request that arrived before the arm was serving. + + A motion is refused outright. Holding it would be worse than silence: + the arm is somewhere unknown, nobody is watching, and the motion would + fire unattended the moment the link came back. + + Everything else is tried on the arm, because a controller that answers + while refusing to serve is exactly what `recover` and `clear` are for, + and refusing them here would leave the one state they exist to escape + with no way out of it. + """ + if call.moves_the_arm: + self._finish_motion(call, FAILED, False, "the arm is not serving yet") + return + self._respond(call, self._attempt(lambda: services.run(self.robot, call.row))) + def _start_position(self) -> None: """Put the arm where INIT_MODE says it should start, if anywhere.""" if self.restarted and self.config.init_mode != "none": diff --git a/tests/conftest.py b/tests/conftest.py index 2517f0e..f7f7279 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -30,11 +30,16 @@ class FakeArm: self.recover_error = None self.block = None self.stopped = False + self.connected = True # --- reads --- def is_connected(self): - return True + return self.connected + + def connect(self): + self.calls.append("connect") + return self.connected def is_ready(self): return self.ready diff --git a/tests/test_lifecycle.py b/tests/test_lifecycle.py index ba66ab7..5e50c83 100644 --- a/tests/test_lifecycle.py +++ b/tests/test_lifecycle.py @@ -6,17 +6,18 @@ import time import pytest from dora_ulite6.config import Config, MotionConfig -from dora_ulite6.control import MOVING, PREEMPTED, READY, Control, ServiceCall +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, **config): +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 @@ -87,6 +88,56 @@ def test_a_first_start_still_runs_init_mode(monkeypatch): 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 ---