refactor node into services over the dora 1.0 contract

Replaces the command/status JSON pair with dora's own service pattern: one request, one response, correlated by the request_id dora puts in the message metadata. Motions take a wait flag, and their lifecycle is published as the arm's state on motion_state rather than as a per-goal stream — the arm is one resource, so a new motion preempts the one in play and a state describes that exactly. motion_state is keyed by request_id and publishes accepted before the control thread reaches the motion. Both exist to close one race: a caller polling a bare moving/idle flag right after its request would read "not moving" and conclude the motion had already finished. Three threads, because the SDK blocks. The control thread carries out one job at a time and may block; the reporter thread only ever reads the report cache, so telemetry keeps flowing through a ten-second move; the stopper thread exists because a stop fits nowhere else — it must not queue behind the motion it aborts, and emergency_stop re-commands the controller for up to three seconds, which is far too long to stop routing a dataflow. The event loop never calls anything that can block, get_state included: that one is allowed there only because it reads the cache. The node is built before the arm threads start, so INIT_MODE cannot move a robot before the process has joined the dataflow, and a restart leaves the arm where it is rather than driving it home unasked. Shutdown halts the arm first — a motion lives in the controller, and dropping the connection does not abort it.
2026-09-12 11:46:59 -03:00
parent eccd9e3c24
commit fdabf7e241
11 changed files with 2089 additions and 1024 deletions
@@ -0,0 +1,231 @@
+"""Non-blocking motions, and a new one taking the place of the old."""
+
+import threading
+
+import pytest
+
+from dora_ulite6.config import Config
+from dora_ulite6.control import (
+    ACCEPTED,
+    DONE,
+    MOVING,
+    PREEMPTED,
+    Control,
+    ServiceCall,
+)
+from tests.conftest import FakeArm, wait_for
+
+
+@pytest.fixture
+def control(monkeypatch):
+    """Start a Control over a fake arm whose motions can be held open."""
+    arms = []
+
+    def build(config):
+        arm = FakeArm(config)
+        arm.block = threading.Event()
+        arms.append(arm)
+        return arm
+
+    monkeypatch.setattr("dora_ulite6.control.ULite6", build)
+    control = Control(Config(telemetry_interval_s=0.01, reconnect_interval_s=0.01))
+    control.arms = arms
+    control.start()
+    wait_for(lambda: control.robot is not None)
+    yield control
+    arms[0].block.set()
+    control.shutdown()
+
+
+class Mail:
+    """Accumulates what the control threads produced, so nothing is missed."""
+
+    def __init__(self, control):
+        self.control = control
+        self.messages = []
+
+    def drain(self):
+        self.messages.extend(self.control.take_messages())
+        return self.messages
+
+    def states(self, request_id):
+        """Return the motion_state lifecycle published for one request, in order."""
+        self.drain()
+        return [
+            m.fields["state"]
+            for m in self.messages
+            if m.output == "motion_state" and m.fields["request_id"] == request_id
+        ]
+
+    def responses(self, request_id):
+        self.drain()
+        return [
+            m.fields
+            for m in self.messages
+            if m.output == "response" and m.metadata.get("request_id") == request_id
+        ]
+
+    def wait_for_state(self, request_id, state):
+        wait_for(lambda: state in self.states(request_id))
+
+
+def motion(request_id, wait=False, service="home", **args):
+    return ServiceCall(request_id=request_id, row={"service": service, **args}, wait=wait)
+
+
+def test_a_non_blocking_motion_is_answered_before_the_arm_stops(control):
+    mail = Mail(control)
+
+    control.start_motion(motion("m1", wait=False))
+    wait_for(lambda: mail.responses("m1"))
+
+    # The caller has its answer while the arm is still held open — the response
+    # was never waiting on the motion, so the motion has not even been reported
+    # as finished.
+    assert not control.arms[0].block.is_set()
+    assert DONE not in mail.states("m1")
+    assert mail.responses("m1")[0] == {
+        "ok": True,
+        "service": "home",
+        "message": "accepted",
+        "accepted": True,
+    }
+
+
+def test_a_non_blocking_motion_reports_completion_only_on_the_state(control):
+    mail = Mail(control)
+
+    control.start_motion(motion("m1", wait=False))
+    mail.wait_for_state("m1", MOVING)
+    control.arms[0].block.set()
+    mail.wait_for_state("m1", DONE)
+
+    assert mail.states("m1") == [ACCEPTED, MOVING, DONE]
+    # Exactly one response, the acknowledgement. A second would break the
+    # one-response-per-request contract the caller correlates by.
+    assert len(mail.responses("m1")) == 1
+
+
+def test_a_blocking_motion_is_answered_only_when_the_arm_stops(control):
+    mail = Mail(control)
+
+    control.start_motion(motion("m1", wait=True))
+    mail.wait_for_state("m1", MOVING)
+
+    assert mail.responses("m1") == []
+
+    control.arms[0].block.set()
+    wait_for(lambda: mail.responses("m1"))
+
+    assert mail.responses("m1")[0]["ok"] is True
+
+
+def test_accepted_is_published_before_the_thread_reaches_the_motion(control):
+    # The race a bare moving/idle flag leaves open: a caller polling right after
+    # its request must not read a state that says nothing is moving.
+    mail = Mail(control)
+    control.arms[0].block = threading.Event()
+
+    control.start_motion(motion("m1", wait=False))
+
+    assert mail.states("m1")[0] == ACCEPTED
+
+
+def test_a_new_motion_preempts_the_one_running(control):
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=False))
+    mail.wait_for_state("m1", MOVING)
+
+    control.start_motion(motion("m2", wait=False, service="move_to", x=1.0, y=2.0, z=3.0))
+
+    mail.wait_for_state("m1", PREEMPTED)
+    assert "stop" in control.arms[0].calls
+    assert mail.states("m1") == [ACCEPTED, MOVING, PREEMPTED]
+
+
+def test_the_preempting_motion_then_runs(control):
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=False))
+    mail.wait_for_state("m1", MOVING)
+
+    control.start_motion(motion("m2", wait=False, service="move_to", x=1.0, y=2.0, z=3.0))
+    mail.wait_for_state("m1", PREEMPTED)
+    control.arms[0].stopped = False
+    control.arms[0].block.set()
+    mail.wait_for_state("m2", DONE)
+
+    # The software stop the preemption caused is cleared before the new move.
+    assert "clear" in control.arms[0].calls
+    assert any(isinstance(call, tuple) and call[0] == "move_to" for call in control.arms[0].calls)
+
+
+def test_a_preempted_blocking_caller_is_released_with_a_failure(control):
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=True))
+    mail.wait_for_state("m1", MOVING)
+
+    control.start_motion(motion("m2", wait=False))
+
+    wait_for(lambda: mail.responses("m1"))
+    answer = mail.responses("m1")[0]
+    assert answer["ok"] is False
+    assert "preempted by m2" in answer["message"]
+
+
+def test_a_motion_still_waiting_its_turn_is_dropped_without_moving_the_arm(control):
+    # Driven straight at the branch rather than through the threads: whether a
+    # preemption lands before or after the control thread picks the motion up is
+    # a race by nature, and only one side of it is this behaviour.
+    mail = Mail(control)
+    call = motion("m2", wait=True, service="move_to", x=1.0, y=2.0, z=3.0)
+    call.preempted_by = "m3"
+
+    control._run_motion(call)
+
+    assert mail.states("m2") == [PREEMPTED]
+    assert mail.responses("m2")[0]["ok"] is False
+    assert "preempted by m3" in mail.responses("m2")[0]["message"]
+    assert not any(isinstance(c, tuple) and c[0] == "move_to" for c in control.arms[0].calls)
+
+
+def test_every_preempted_request_is_answered_exactly_once(control):
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=True))
+    mail.wait_for_state("m1", MOVING)
+    control.start_motion(motion("m2", wait=True))
+    mail.wait_for_state("m1", PREEMPTED)
+    # m2 may or may not have started by now; either way m3 takes its place, and
+    # what must hold is that nobody is left without an answer.
+    control.start_motion(motion("m3", wait=True))
+    control.arms[0].block.set()
+
+    for request_id in ("m1", "m2", "m3"):
+        wait_for(lambda rid=request_id: mail.responses(rid))
+        assert len(mail.responses(request_id)) == 1, request_id
+
+
+def test_a_preemption_that_stopped_nothing_still_clears_for_the_one_that_did(control):
+    # A preemption of a motion still waiting its turn stops nothing, and must
+    # not swallow the pending clear owed by an earlier one that did.
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=False))
+    mail.wait_for_state("m1", MOVING)
+
+    control.start_motion(motion("m2", wait=False))  # stops m1
+    mail.wait_for_state("m1", PREEMPTED)
+    control.start_motion(motion("m3", wait=False))  # may stop nothing
+    control.arms[0].block.set()
+
+    mail.wait_for_state("m3", DONE)
+    assert "clear" in control.arms[0].calls
+
+
+def test_a_service_that_does_not_move_the_arm_never_preempts(control):
+    mail = Mail(control)
+    control.start_motion(motion("m1", wait=False))
+    mail.wait_for_state("m1", MOVING)
+
+    control.submit(ServiceCall(request_id="s1", row={"service": "clear"}))
+
+    assert PREEMPTED not in mail.states("m1")
+    assert "stop" not in control.arms[0].calls