dora_ulite6/__init__.py

@@ -1 +1 @@
-"""Dora ULite6 node package."""
+"""Dora node for a UFactory Lite6 arm: telemetry out, commands in."""

dora_ulite6/control.py

@@ -0,0 +1,552 @@
+"""The threads that own the arm, and the mailboxes the event loop drains.
+
+**Nothing outside this file may call into the driver**, with two exceptions
+written to be safe from another thread: :meth:`ULite6.stop`, and
+:meth:`ULite6.read_report`, which reads the controller's report cache rather
+than the wire.
+
+There are two threads because a motion must not stop the arm being watched. The
+xArm SDK blocks: a move waits out the motion, and a read from a controller whose
+cable was pulled blocks until its own socket timeout. So:
+
+- The **control thread** runs one job at a time and is allowed to block. It is
+  where every service is carried out, including the motions that take seconds.
+- The **reporter thread** never blocks. It reads the report cache at a fixed
+  cadence, so `tcp_pose` and `joint_pose` keep flowing right through a ten-second
+  move — which is the only way a caller can follow one, since the thread that
+  started it is inside it until it ends.
+- The **stopper thread** exists only to call `stop`. It cannot be the control
+  thread (a stop must not queue behind the motion it aborts) and it must not be
+  the event loop: the SDK's `emergency_stop` re-commands the controller until it
+  confirms, for up to three seconds, and a dataflow router cannot go away for
+  three seconds. Requests to it coalesce — several stops in a row are one stop.
+
+The dora event loop is a third party to both: it drains mailboxes and publishes,
+and never waits for the arm.
+"""
+
+import queue
+import threading
+import time
+from dataclasses import dataclass, field
+from typing import Any
+
+from dora_ulite6 import services
+from dora_ulite6.config import Config
+from dora_ulite6.driver import ULite6
+from dora_ulite6.errors import ArmNeedsHomingError, RobotUnavailableError
+from dora_ulite6.log import log
+from dora_ulite6.pose import RobotPose, with_tool_offset
+
+# The states the node reports on `robot_state`, in the order a healthy start
+# walks through them.
+CONNECTING = "connecting"
+READY = "ready"
+MOVING = "moving"
+RECOVERING = "recovering"
+UNAVAILABLE = "unavailable"
+
+
+# The motion lifecycle published on `motion_state`. `accepted` exists precisely
+# to close the race a bare moving/idle flag leaves open: between a caller being
+# told its request was taken and the control thread reaching it, a two-valued
+# state still reads "not moving", and a caller polling for completion would
+# conclude the motion it just asked for had already finished.
+ACCEPTED = "accepted"
+DONE = "done"
+FAILED = "failed"
+PREEMPTED = "preempted"
+
+
+@dataclass
+class ServiceCall:
+    """One service request waiting on the control thread."""
+
+    request_id: str
+    row: dict
+    # Whether the caller is held until the arm stops. False means the response
+    # was already sent as an acknowledgement, and completion is only reported on
+    # `motion_state` — which is what keeps it one response per request.
+    wait: bool = True
+    # Set when a newer motion took this one's place, before or during its run.
+    preempted_by: str | None = None
+
+    @property
+    def service(self) -> str:
+        """The service this request names."""
+        return self.row.get(services.SERVICE, "")
+
+    @property
+    def moves_the_arm(self) -> bool:
+        """Whether carrying this out drives the arm, and so preempts and is preemptable."""
+        return self.service in services.MOVES_THE_ARM
+
+
+@dataclass
+class Jog:
+    """One velocity command. Not a service: nobody waits for an answer."""
+
+    velocity: list[float]
+
+
+@dataclass
+class Telemetry:
+    """One reading of the arm: joint angles in degrees, tool-tip pose in mm and degrees."""
+
+    joints: list[float]
+    tip: RobotPose
+    timestamp_ns: int
+
+
+@dataclass
+class Message:
+    """Something to publish: an output id, its fields, and its correlation metadata."""
+
+    output: str
+    fields: dict[str, Any]
+    metadata: dict[str, str] = field(default_factory=dict)
+
+
+class Control:
+    """Owns the arm and hands the event loop what there is to publish."""
+
+    def __init__(self, config: Config, restarted: bool = False):
+        self.config = config
+        # A restarted node inherits an arm that is wherever it was left. Driving
+        # it to INIT_MODE because the process came back is a motion nobody asked
+        # for, so a restart skips it.
+        self.restarted = restarted
+        self.robot: ULite6 | None = None
+        self._jobs: queue.Queue[ServiceCall | Jog] = queue.Queue()
+        self._lock = threading.Lock()
+        self._telemetry: Telemetry | None = None
+        self._outbox: list[Message] = []
+        self._state = CONNECTING
+        self._state_changes: list[str] = [CONNECTING]
+        # The motion the control thread is inside right now, and whether the
+        # last preemption left a software stop for the next motion to clear.
+        self._running_motion: ServiceCall | None = None
+        self._stopped_for_preempt = False
+        # Set by anyone who needs the arm halted; the stopper thread drains it.
+        self._stop_wanted = threading.Event()
+        # Whether a velocity command is currently keeping the arm moving, and
+        # when the last one arrived — the controller expires them on its own.
+        self._jogging = False
+        self._last_jog = 0.0
+        # Whether the arm has been reached once and put where INIT_MODE wants it.
+        self._started = False
+        # Set to ask the threads to finish. The sense matters: this is waited on
+        # as the threads' sleep, and an event that is *set* while running makes
+        # every wait() return at once — a sleep that never sleeps, which is a
+        # busy loop rather than a cadence.
+        self._stopping = threading.Event()
+        self._control = threading.Thread(target=self._run_control, name="ulite6-control", daemon=True)
+        self._reporter = threading.Thread(target=self._run_reporter, name="ulite6-reporter", daemon=True)
+        self._stopper = threading.Thread(target=self._run_stopper, name="ulite6-stopper", daemon=True)
+
+    def start(self) -> None:
+        """Start the three threads; they reach the arm themselves, so this never blocks."""
+        self._control.start()
+        self._reporter.start()
+        self._stopper.start()
+
+    def shutdown(self) -> bool:
+        """Halt the arm, stop the threads, and say whether they really finished.
+
+        Halting first is the point. A motion lives in the *controller*, not in
+        this process: dropping the connection does not abort it, so a node that
+        exits mid-move leaves an arm travelling with nobody left to stop it. The
+        stop is commanded here on the caller's thread and waited for, because by
+        now the event loop has finished routing and there is nothing left to
+        starve.
+
+        Returns False if the control thread is still inside the SDK afterwards,
+        which is the caller's cue to leave without waiting for it.
+        """
+        if self.robot is not None:
+            self._attempt(self.robot.stop)
+        self._stopping.set()
+        self._stop_wanted.set()
+        self._stopper.join(timeout=1.0)
+        self._reporter.join(timeout=1.0)
+        self._control.join(timeout=self.config.motion.ready_timeout_s)
+        if self._control.is_alive():
+            log("the control thread is still inside the SDK; leaving it behind")
+            return False
+        return True
+
+    # --- what the event loop puts in ---
+
+    def submit(self, job: ServiceCall | Jog) -> None:
+        """Queue work for the control thread. Returns immediately."""
+        self._jobs.put(job)
+
+    def request_stop(self, request_id: str = "") -> None:
+        """Ask for the arm to be halted, and come straight back.
+
+        Answering before the arm has actually stopped is deliberate: the reply
+        says the stop was commanded, and whether it took is reported where every
+        other outcome is, on `motion_state` and `robot_state`. Waiting here
+        instead would hand the caller a better answer at the price of the whole
+        dataflow, for up to three seconds.
+        """
+        self._stop_wanted.set()
+        if request_id:
+            self._respond(
+                ServiceCall(request_id=request_id, row={services.SERVICE: services.STOP}),
+                (True, {"message": "stop commanded"}),
+            )
+
+    def _preempt_motions(self, by: str) -> None:
+        """Mark the running motion and any still waiting, and halt the arm if one was moving."""
+        with self._lock:
+            running = self._running_motion
+            waiting = [
+                job
+                for job in list(self._jobs.queue)
+                if isinstance(job, ServiceCall) and job.moves_the_arm and job.preempted_by is None
+            ]
+            for job in [running, *waiting]:
+                if job is not None:
+                    job.preempted_by = by
+            # The next motion to actually run clears the software stop this is
+            # about to cause. Only ever set here, never unset: preempting a
+            # motion that had not started yet stops nothing, and must not clear a
+            # flag left by an earlier preemption that nothing has consumed — that
+            # would send the next motion at an arm still in its stopped state,
+            # where it would be refused the moment it was commanded.
+            self._stopped_for_preempt = self._stopped_for_preempt or running is not None
+        if running is not None:
+            log(f"{running.service} ({running.request_id}) preempted by {by}")
+            self.request_stop()
+
+    def start_jog(self, velocity: list[float]) -> None:
+        """Take one velocity command, preempting a motion the first time it moves the arm.
+
+        A jog arrives many times a second, so everything here happens on the
+        transitions only: a stream of them is one preemption and one state
+        change, not hundreds.
+        """
+        moving = any(velocity)
+        if moving and not self._jogging:
+            self._preempt_motions("jog")
+            self._set_state(MOVING)
+        with self._lock:
+            self._jogging = moving
+            self._last_jog = time.monotonic()
+        if not moving:
+            self._set_state(READY)
+        self._jobs.put(Jog(velocity=velocity))
+
+    def start_motion(self, call: ServiceCall) -> None:
+        """Take a motion, preempting whatever motion was already in play.
+
+        The arm is one resource: there is never more than one motion, so a new
+        one replaces the old rather than queueing behind it. The running motion
+        is cut short through the arm — the same stop a caller can send — and any
+        motion still waiting its turn is dropped without ever moving anything.
+
+        Each preempted request is answered exactly once, as every request is: if
+        its caller is still waiting, by its response; if it was a `wait: false`
+        request, it was acknowledged when it arrived, and the preemption shows
+        only on `motion_state`.
+        """
+        self._preempt_motions(call.request_id)
+        self._motion_state(call, ACCEPTED, "queued")
+        if not call.wait:
+            self._respond(call, (True, {"message": "accepted", "accepted": True}))
+        self._jobs.put(call)
+
+    def post(self, message: Message) -> None:
+        """Add a message for the event loop to publish on the next tick.
+
+        The loop uses this to answer a request it refused itself, so a malformed
+        message is still answered under its own correlation id without the arm
+        threads being troubled by it.
+        """
+        self._post(message)
+
+    def call_now(self, request_id: str, row: dict) -> None:
+        """Run a service on the calling thread, for the ones that must not queue.
+
+        Only `stop` takes this path: queued, it would sit behind the very motion
+        it exists to abort. It is safe here because the driver method it reaches
+        is the one written for another thread.
+        """
+        call = ServiceCall(request_id=request_id, row=row)
+        if self.robot is None:
+            self._respond(call, (False, {"message": "the arm has not been reached yet"}))
+            return
+        self._respond(call, self._attempt(lambda: services.run(self.robot, row)))
+
+    # --- what the event loop drains ---
+
+    def take_telemetry(self) -> Telemetry | None:
+        """Return the newest reading and clear it, so a tick never republishes a stale pose."""
+        with self._lock:
+            telemetry, self._telemetry = self._telemetry, None
+            return telemetry
+
+    def take_messages(self) -> list[Message]:
+        """Return every response recorded since the last call."""
+        with self._lock:
+            outbox, self._outbox = self._outbox, []
+            return outbox
+
+    def take_state_changes(self) -> list[str]:
+        """Return every state transition since the last call."""
+        with self._lock:
+            changes, self._state_changes = self._state_changes, []
+            return changes
+
+    def _post(self, message: Message) -> None:
+        with self._lock:
+            self._outbox.append(message)
+
+    # --- the control thread ---
+
+    def _run_control(self) -> None:
+        while not self._stopping.is_set():
+            if not self._started:
+                self._start_up()
+                continue
+            try:
+                job = self._jobs.get(timeout=0.1)
+            except queue.Empty:
+                continue
+            if isinstance(job, Jog):
+                self._run_jog(job)
+            else:
+                self._run_service(job)
+
+    def _start_up(self) -> None:
+        """Reach an arm that answers, put it where INIT_MODE says, and start serving.
+
+        The driver is built once and then retried through its own ``connect()``,
+        which reuses the SDK handle: rebuilding it on every attempt would leak a
+        socket and its threads for every second an absent arm stays absent.
+        """
+        self._set_state(CONNECTING)
+        if self.robot is None:
+            self.robot = ULite6(self.config)
+        try:
+            if not self.robot.is_connected() and not self.robot.connect():
+                raise RobotUnavailableError(f"robot at {self.config.robot_ip} is not reachable")
+            self._start_position()
+        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)
+            return
+        self._started = True
+        self._set_state(READY)
+
+    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":
+            log(f"restarted: leaving the arm where it is rather than running INIT_MODE={self.config.init_mode}")
+            return
+        if self.config.init_mode == "home":
+            log("moving to home")
+            self.robot.go_home()
+        elif self.config.init_mode == "pose":
+            log(f"moving to the initial pose {self.config.init_pose.as_list()}")
+            self.robot.move_to(
+                with_tool_offset(self.config.init_pose, -self.config.tool_offset_mm),
+                self.config.init_speed,
+            )
+
+    def _run_service(self, call: ServiceCall) -> None:
+        """Carry out one service and post its response."""
+        if call.moves_the_arm:
+            self._run_motion(call)
+            return
+        self._respond(call, self._attempt(lambda: services.run(self.robot, call.row)))
+
+    def _run_motion(self, call: ServiceCall) -> None:
+        """Carry out one motion, reporting its whole life on `motion_state`.
+
+        Blocks for as long as the arm takes. `robot_state` says `moving`
+        meanwhile, so a caller watching it can tell a slow service from a stalled
+        one, and `tcp_pose` keeps arriving because the reporter thread is not the
+        one in here.
+        """
+        if call.preempted_by is not None:
+            # Replaced while it waited its turn; it never moved anything.
+            self._finish_motion(call, PREEMPTED, False, f"preempted by {call.preempted_by}")
+            return
+        with self._lock:
+            self._running_motion = call
+            clear_first = self._stopped_for_preempt
+            self._stopped_for_preempt = False
+        self._set_state(MOVING)
+        self._motion_state(call, MOVING, "")
+        if self._leave_jog():
+            # A pose command would switch the controller out of velocity mode
+            # behind the driver's back, leaving its own bookkeeping saying the
+            # arm is still jogging. Leaving through the front door keeps them
+            # agreed.
+            self._attempt(lambda: self.robot.set_cartesian_velocity([0.0] * 6))
+        if clear_first:
+            # Preempting leaves the controller in its software-stopped state.
+            # The driver's own set_state(0) would probably be enough; clearing
+            # first is cheap and does not depend on that being true.
+            self._attempt(self.robot.clear)
+        outcome = self._attempt(lambda: services.run(self.robot, call.row))
+        with self._lock:
+            self._running_motion = None
+            preempted_by = call.preempted_by
+        ok, fields = outcome
+        if preempted_by is not None:
+            # A preempted move fails through MotionInterruptedError like any
+            # aborted one; only the flag says who cut it short.
+            self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
+        else:
+            self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
+        if self._state == MOVING:
+            self._set_state(READY)
+
+    def _leave_jog(self) -> bool:
+        """Clear the jogging flag, reporting whether the arm was in fact jogging."""
+        with self._lock:
+            was_jogging, self._jogging = self._jogging, False
+        return was_jogging
+
+    def _finish_motion(self, call: ServiceCall, state: str, ok: bool, message: str, fields=None) -> None:
+        """Publish a motion's end, and answer its caller if one is still waiting."""
+        self._motion_state(call, state, message)
+        if call.wait:
+            self._respond(call, (ok, dict(fields or {"message": message})))
+
+    def _motion_state(self, call: ServiceCall, state: str, message: str) -> None:
+        """Publish where a motion stands.
+
+        The request_id rides in the *fields*, not the metadata: metadata carrying
+        a request_id means "this is the answer to that request", and this is a
+        broadcast of the arm's state, which anyone may read. Keeping it in the
+        fields is also what lets a caller polling `get_state` see the same value
+        as one watching the topic.
+        """
+        self._post(
+            Message(
+                output="motion_state",
+                fields={
+                    "request_id": call.request_id,
+                    "service": call.service,
+                    "state": state,
+                    "message": message,
+                },
+            )
+        )
+
+    def _run_jog(self, jog: Jog) -> None:
+        """Command one velocity. Nothing is published: a jog has no answer."""
+        def command() -> dict:
+            self.robot.set_cartesian_velocity(jog.velocity)
+            return {"message": "jogging"}
+
+        ok, fields = self._attempt(command)
+        if not ok:
+            log(f"jog refused: {fields['message']}")
+
+    def _respond(self, call: ServiceCall, outcome: tuple[bool, dict]) -> None:
+        """Post the one response this request gets."""
+        ok, fields = outcome
+        fields.setdefault("message", "")
+        self._post(
+            Message(
+                output="response",
+                fields={"ok": ok, "service": call.service, **fields},
+                metadata={"request_id": call.request_id},
+            )
+        )
+
+    def _attempt(self, work) -> tuple[bool, dict]:
+        """Run ``work``, turning whatever it raises into a failed response."""
+        try:
+            return True, work()
+        except ArmNeedsHomingError as error:
+            return False, {"message": f"{error}; call `home_and_clear` once the area is clear"}
+        except RobotUnavailableError as error:
+            self._go_unavailable(error)
+            return False, {"message": str(error)}
+        except Exception as error:  # noqa: BLE001 — a failed request must never take the thread down
+            return False, {"message": str(error)}
+
+    def _go_unavailable(self, error: Exception) -> None:
+        """Report the arm as unusable and try once to bring it back.
+
+        A single attempt, not a retry loop: the thread has jobs to serve, and the
+        reporter comes round again in milliseconds.
+        """
+        self._set_state(RECOVERING)
+        log(f"the arm is unusable ({error}); trying to recover")
+        try:
+            self.robot.recover()
+        except RobotUnavailableError as recovery_error:
+            log(f"recovery failed: {recovery_error}")
+            self._set_state(UNAVAILABLE)
+            return
+        self._set_state(READY)
+
+    # --- the stopper thread ---
+
+    def _run_stopper(self) -> None:
+        """Halt the arm whenever anyone asks, and never make them wait for it.
+
+        One stop per wake-up however many asked, which is what a stop means:
+        several callers wanting the arm halted want the same single thing.
+        """
+        while not self._stopping.is_set():
+            if not self._stop_wanted.wait(timeout=0.1):
+                continue
+            self._stop_wanted.clear()
+            if self._stopping.is_set():
+                return
+            if self.robot is not None:
+                self._attempt(self.robot.stop)
+
+    def _set_state(self, state: str) -> None:
+        """Record a state, queueing it for publication only when it is a change."""
+        with self._lock:
+            if state == self._state:
+                return
+            self._state = state
+            self._state_changes.append(state)
+        log(f"state: {state}")
+
+    # --- the reporter thread ---
+
+    def _expire_jog(self) -> None:
+        """Stop calling the arm jogging once the controller has expired the command.
+
+        A jog lives on the controller's own TTL, so a sender that goes quiet
+        leaves an arm that stopped moving a moment later. Nothing tells us; the
+        clock does.
+        """
+        with self._lock:
+            if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
+                return
+            self._jogging = False
+        self._set_state(READY)
+
+    def _run_reporter(self) -> None:
+        """Publish telemetry, whatever the control thread is doing.
+
+        Never touches anything that can block, so it keeps running through a
+        ten-second move — which is what lets a caller follow a motion whose
+        response will not arrive until it ends.
+        """
+        while not self._stopping.is_set():
+            self._stopping.wait(self.config.telemetry_interval_s)
+            if self.robot is None:
+                continue
+            self._expire_jog()
+            reading = self.robot.read_report()
+            if reading is None:
+                continue
+            joints, flange = reading
+            tip = with_tool_offset(flange, self.config.tool_offset_mm)
+            with self._lock:
+                self._telemetry = Telemetry(joints, tip, time.time_ns())
+

dora_ulite6/payload.py

@@ -0,0 +1,91 @@
+"""Reading arguments out of the Arrow row that carries a request.
+
+Every request arrives as a one-row Arrow struct, and each service
+brings its own schema — dora allows that on an input carrying a correlation id,
+so nothing here assumes a fixed set of fields. `as_py()` hands back a plain dict
+and these helpers read it, naming what was missing or unusable rather than
+letting a `None` reach the arm.
+"""
+
+from typing import Any
+
+from dora_ulite6.pose import RobotPose
+
+
+class PayloadError(Exception):
+    """The request could not be understood, so nothing was asked of the arm."""
+
+
+def read_row(value) -> dict[str, Any]:
+    """Return the single struct row of ``value`` as a dict.
+
+    Raises
+    ------
+    PayloadError
+        The message was empty, or was not a struct this node can read.
+    """
+    if value is None or len(value) == 0:
+        raise PayloadError("the message carries no data")
+    row = value[0].as_py()
+    if not isinstance(row, dict):
+        raise PayloadError(f"expected a struct with named fields, got {type(row).__name__}")
+    return row
+
+
+def name_of(row: dict, field: str, known) -> str:
+    """Return the operation ``row`` names, or raise listing the ones that exist."""
+    name = row.get(field)
+    if not isinstance(name, str) or name not in known:
+        raise PayloadError(f"unknown {field} {name!r}; available: {', '.join(sorted(known))}")
+    return name
+
+
+def number(row: dict, field: str) -> float:
+    """Return a required numeric field."""
+    if field not in row or row[field] is None:
+        raise PayloadError(f"missing `{field}`")
+    try:
+        return float(row[field])
+    except (TypeError, ValueError):
+        raise PayloadError(f"`{field}` must be a number, not {row[field]!r}") from None
+
+
+def optional_number(row: dict, field: str, default: float | None) -> float | None:
+    """Return a numeric field, or ``default`` when it is absent or null."""
+    if field not in row or row[field] is None:
+        return default
+    try:
+        return float(row[field])
+    except (TypeError, ValueError):
+        raise PayloadError(f"`{field}` must be a number, not {row[field]!r}") from None
+
+
+def flag(row: dict, field: str, default: bool) -> bool:
+    """Return a boolean field, or ``default`` when it is absent or null."""
+    value = row.get(field)
+    return default if value is None else bool(value)
+
+
+def numbers(row: dict, field: str, count: int | None = None) -> list[float]:
+    """Return a required list of numbers, optionally of an exact length."""
+    values = row.get(field)
+    if not isinstance(values, list) or not values:
+        raise PayloadError(f"`{field}` must be a non-empty list of numbers")
+    if count is not None and len(values) != count:
+        raise PayloadError(f"`{field}` must hold {count} numbers, got {len(values)}")
+    try:
+        return [float(value) for value in values]
+    except (TypeError, ValueError):
+        raise PayloadError(f"`{field}` must hold only numbers") from None
+
+
+def pose(row: dict) -> RobotPose:
+    """Read a tool-tip pose, defaulting the orientation to tool-down."""
+    return RobotPose(
+        x=number(row, "x"),
+        y=number(row, "y"),
+        z=number(row, "z"),
+        roll=optional_number(row, "roll", 180.0),
+        pitch=optional_number(row, "pitch", 0.0),
+        yaw=optional_number(row, "yaw", 0.0),
+    )

dora_ulite6/services.py

@@ -0,0 +1,167 @@
+"""Every operation the node serves, and what each one does to the arm.
+
+A service is one request and one response, correlated by the `request_id` dora
+puts in the message metadata. That is the node's whole control surface: there is
+no second protocol for the slow ones.
+
+Two groups behave differently enough to be worth naming here:
+
+- The motions in :data:`MOVES_THE_ARM` take as long as the arm does. A caller
+  chooses with `wait` whether to be held until the arm stops or answered at
+  once; either way the motion's progress is on `motion_state`. A new motion
+  preempts one already running, so there is never more than one in play.
+- The services in :data:`IMMEDIATE` are run by the event loop rather than queued,
+  because queued they would sit behind a motion — which is exactly when they are
+  wanted. Putting one there is a promise that it never blocks: `get_state` reads
+  the controller's report cache rather than the wire, which is what makes it
+  answerable *during* a motion.
+
+`stop` is a service to a caller but has no handler here at all. It cannot queue
+(it exists to abort what is at the head of the queue) and it cannot run on the
+event loop either: the SDK's `emergency_stop` re-commands the controller for up
+to three seconds, and three seconds is far too long to stop routing a dataflow.
+It gets a thread of its own — see :mod:`dora_ulite6.control`.
+"""
+
+from typing import Any
+
+from dora_ulite6 import payload
+from dora_ulite6.driver import ULite6
+from dora_ulite6.pose import with_tool_offset
+
+# Field naming the service in a request row.
+SERVICE = "service"
+
+
+def run(robot: ULite6, row: dict) -> dict[str, Any]:
+    """Carry out the service ``row`` names and return the response fields.
+
+    ``row`` must name a service with a handler, which `stop` deliberately has
+    not; the event loop routes that one before it ever reaches here.
+
+    Blocks for as long as the service takes — seconds, for a motion.
+
+    Raises
+    ------
+    PayloadError
+        The row named no known service, or its arguments were unusable.
+    RobotUnavailableError, UnreachablePoseError, EffectorUnsupportedError
+        The arm refused; the caller turns it into a failed response.
+    """
+    return SERVICES[payload.name_of(row, SERVICE, SERVICES)](robot, row)
+
+
+# --- motions: these block, and they move a real arm ---
+
+
+def _move_to(robot: ULite6, row: dict) -> dict:
+    tip = payload.pose(row)
+    # The request names where the tool tip should end up; the controller only
+    # takes flange poses, so the tool's own reach comes back off here.
+    robot.move_to(
+        with_tool_offset(tip, -robot.config.tool_offset_mm),
+        payload.optional_number(row, "speed", None),
+    )
+    return {"message": f"the tool is at ({tip.x:.0f}, {tip.y:.0f}, {tip.z:.0f} mm)"}
+
+
+def _move_joints(robot: ULite6, row: dict) -> dict:
+    angles = payload.numbers(row, "angles")
+    robot.set_joint_angles(angles, payload.optional_number(row, "speed", None))
+    return {"message": f"the joints are at {angles}"}
+
+
+def _home(robot: ULite6, row: dict) -> dict:
+    robot.go_home()
+    return {"message": "the arm is home"}
+
+
+def _home_and_clear(robot: ULite6, row: dict) -> dict:
+    robot.home_and_clear()
+    return {"message": "the arm was cleared and driven home"}
+
+
+# --- everything that answers immediately ---
+
+
+def _clear(robot: ULite6, row: dict) -> dict:
+    robot.clear()
+    return {"message": "the controller error is cleared"}
+
+
+def _recover(robot: ULite6, row: dict) -> dict:
+    robot.recover()
+    return {"message": "the arm is usable again"}
+
+
+def _freedrive(robot: ULite6, row: dict) -> dict:
+    enabled = payload.flag(row, "enabled", True)
+    robot.set_freedrive(enabled)
+    return {"message": f"freedrive {'on' if enabled else 'off'}"}
+
+
+def _grasp(robot: ULite6, row: dict) -> dict:
+    robot.engage_tool()
+    return {"message": "the tool is holding"}
+
+
+def _release(robot: ULite6, row: dict) -> dict:
+    robot.release_tool()
+    return {"message": "the tool has let go"}
+
+
+def _get_state(robot: ULite6, row: dict) -> dict:
+    """Read the arm and hand back everything a caller might otherwise poll for.
+
+    Reads the report cache, not the wire. That is what lets this run on the event
+    loop and answer *during* a motion — the point of polling at all — where a
+    `get_position()` round trip would block behind the move on the same socket.
+
+    An arm the report cache cannot speak for answers `connected: false` rather
+    than handing back the pose it held before the cable was pulled.
+    """
+    reading = robot.read_report()
+    state = {
+        "connected": reading is not None,
+        "ready": robot.is_ready(),
+        "tool_engaged": robot.tool_engaged,
+        "freedrive": robot.freedrive_on,
+        "pose": [],
+        "joints": [],
+    }
+    if reading is None:
+        return {"message": "not connected", **state}
+    joints, flange = reading
+    state["pose"] = with_tool_offset(flange, robot.config.tool_offset_mm).as_list()
+    state["joints"] = joints
+    return {"message": "ready" if state["ready"] else "not ready", **state}
+
+
+SERVICES = {
+    "move_to": _move_to,
+    "move_joints": _move_joints,
+    "home": _home,
+    "home_and_clear": _home_and_clear,
+    "clear": _clear,
+    "recover": _recover,
+    "freedrive": _freedrive,
+    "grasp": _grasp,
+    "release": _release,
+    "get_state": _get_state,
+}
+
+# The services that drive the arm across the table. They are what `robot_state`
+# reports as `moving`, and what a `stop` is there to interrupt.
+MOVES_THE_ARM = frozenset({"move_to", "move_joints", "home", "home_and_clear"})
+
+# Served on the event loop rather than the control thread. See the module note.
+# Adding to this set is a promise that the service never blocks and is safe from
+# a thread other than the control one — check the driver method before you do.
+IMMEDIATE = frozenset({"get_state"})
+
+# Named by callers and accepted, but handled by neither of the above. See the
+# module note for why it has a thread to itself.
+STOP = "stop"
+
+# Every name a request may carry.
+NAMES = frozenset(SERVICES) | {STOP}

tests/conftest.py

@@ -0,0 +1,164 @@
+"""Shared fakes. Nothing here needs a robot, a dataflow or a daemon."""
+
+import time
+
+import pytest
+
+from dora_ulite6.pose import RobotPose
+
+TIMEOUT = 2.0
+
+
+class FakeArm:
+    """Stands in for the driver: records what it was asked and answers instantly.
+
+    ``block`` lets a test hold a motion open the way a real one does, which is
+    the only way to observe feedback and cancellation at all.
+    """
+
+    def __init__(self, config):
+        self.config = config
+        self.calls = []
+        self.ready = True
+        self.tool_engaged = False
+        self.freedrive_on = False
+        self.pose = RobotPose(300, 0, 250)
+        self.joints = [0.0] * 6
+        self.read_error = None
+        self.move_error = None
+        self.command_error = None
+        self.recover_error = None
+        self.block = None
+        self.stopped = False
+
+    # --- reads ---
+
+    def is_connected(self):
+        return True
+
+    def is_ready(self):
+        return self.ready
+
+    def read_report(self):
+        # A report cache that cannot speak for the arm returns nothing, the way
+        # the real one does when the link is down.
+        if self.read_error:
+            return None
+        return list(self.joints), self.pose
+
+    def get_joint_angles(self):
+        if self.read_error:
+            raise self.read_error
+        return list(self.joints)
+
+    def get_current_position(self):
+        if self.read_error:
+            raise self.read_error
+        return self.pose
+
+    # --- motion ---
+
+    def _move(self, what):
+        self.calls.append(what)
+        if self.block is not None:
+            # Hold the "motion" open until the test releases it or stops the arm.
+            deadline = time.monotonic() + TIMEOUT
+            while not self.block.is_set() and not self.stopped and time.monotonic() < deadline:
+                time.sleep(0.005)
+        if self.stopped:
+            from dora_ulite6.errors import MotionInterruptedError
+
+            raise MotionInterruptedError("aborted by stop")
+        if self.move_error:
+            raise self.move_error
+
+    def go_home(self):
+        self._move("go_home")
+
+    def home_and_clear(self):
+        self._move("home_and_clear")
+
+    def move_to(self, pose, speed=None):
+        self._move(("move_to", pose.as_list(), speed))
+
+    def set_joint_angles(self, angles, speed=None):
+        self._move(("set_joint_angles", angles, speed))
+
+    def set_cartesian_velocity(self, velocity):
+        self.calls.append(("jog", velocity))
+
+    def stop(self):
+        self.calls.append("stop")
+        self.stopped = True
+
+    # --- the rest ---
+
+    def clear(self):
+        self.calls.append("clear")
+        self.stopped = False
+
+    def recover(self):
+        self.calls.append("recover")
+        if self.recover_error:
+            raise self.recover_error
+        self.read_error = None
+
+    def engage_tool(self):
+        self.calls.append("engage_tool")
+        self.tool_engaged = True
+
+    def release_tool(self):
+        self.calls.append("release_tool")
+        self.tool_engaged = False
+
+    def set_freedrive(self, enabled):
+        self.calls.append(("set_freedrive", enabled))
+        if self.command_error:
+            raise self.command_error
+        self.freedrive_on = enabled
+
+
+class RecordingNode:
+    """A dora node that records outputs *with* their metadata.
+
+    `dora.testing.MockNode` drops the metadata dict on `send_output`, and the
+    metadata is precisely what this node's service and action patterns are built
+    on — the request_id and goal_id a caller correlates by. So the tests record
+    it themselves.
+    """
+
+    def __init__(self, events=()):
+        self._events = iter(list(events))
+        self.sent = []
+
+    def is_restart(self):
+        return False
+
+    def __iter__(self):
+        return self
+
+    def __next__(self):
+        return next(self._events)
+
+    def send_output(self, output_id, data, metadata=None):
+        self.sent.append((output_id, data, dict(metadata or {})))
+
+    def outputs(self, output_id):
+        """Return every message sent on ``output_id`` as (row, metadata) pairs."""
+        return [(data[0].as_py(), metadata) for name, data, metadata in self.sent if name == output_id]
+
+
+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 {})}
+
+
+def wait_for(predicate):
+    """Poll until ``predicate`` holds, or fail the test rather than hang it."""
+    deadline = time.monotonic() + TIMEOUT
+    while time.monotonic() < deadline:
+        result = predicate()
+        if result:
+            return result
+        time.sleep(0.005)
+    pytest.fail("the control threads never reached the expected state")

tests/test_lifecycle.py

@@ -0,0 +1,241 @@
+"""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, Control, ServiceCall
+from dora_ulite6.pose import RobotPose
+from tests.conftest import FakeArm, wait_for
+
+
+def build(monkeypatch, **config):
+    """Return a started Control over a fake arm, and the arms it built."""
+    arms = []
+
+    def make(cfg):
+        arm = FakeArm(cfg)
+        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()
+
+
+# --- 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_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"

tests/test_loop.py

@@ -0,0 +1,215 @@
+"""The dora event loop: routing, correlation, and refusing without touching the arm."""
+
+import pyarrow as pa
+import pytest
+
+from dora_ulite6 import main
+from dora_ulite6.config import Config
+from dora_ulite6.control import Message, ServiceCall, Telemetry
+from dora_ulite6.pose import RobotPose
+from tests.conftest import RecordingNode, input_event
+
+
+class SpyControl:
+    """Records what the loop hands the arm threads, without starting any."""
+
+    def __init__(self):
+        self.submitted = []
+        self.motions = []
+        self.called_now = []
+        self.stops = []
+        self.jogs = []
+        self.shutdowns = []
+        self.posted = []
+        self._telemetry = None
+        self._states = []
+
+    def submit(self, job):
+        self.submitted.append(job)
+
+    def start_motion(self, call):
+        self.motions.append(call)
+
+    def call_now(self, request_id, row):
+        self.called_now.append((request_id, row))
+
+    def request_stop(self, request_id=""):
+        self.stops.append(request_id)
+
+    def start_jog(self, velocity):
+        self.jogs.append(velocity)
+
+    def shutdown(self):
+        self.shutdowns.append(True)
+        return True
+
+    def post(self, message):
+        self.posted.append(message)
+
+    def take_messages(self):
+        messages, self.posted = self.posted, []
+        return messages
+
+    def take_state_changes(self):
+        states, self._states = self._states, []
+        return states
+
+    def take_telemetry(self):
+        telemetry, self._telemetry = self._telemetry, None
+        return telemetry
+
+
+def row(**fields):
+    return pa.array([fields])
+
+
+def handle(event, control=None):
+    """Run one event through the loop and return the node it wrote to."""
+    node = RecordingNode()
+    main._handle(node, control or SpyControl(), event)
+    return node
+
+
+def test_a_service_request_is_queued_with_its_request_id():
+    control = SpyControl()
+
+    main._handle(RecordingNode(), control, input_event("request", row(service="clear"), {"request_id": "r1"}))
+
+    assert isinstance(control.submitted[0], ServiceCall)
+    assert control.submitted[0].request_id == "r1"
+
+
+def test_stop_goes_to_the_stopper_rather_than_the_queue_or_the_loop():
+    # It must not queue behind the motion it aborts, and must not run here
+    # either: the SDK spends up to three seconds confirming a stop.
+    control = SpyControl()
+
+    main._handle(RecordingNode(), control, input_event("request", row(service="stop"), {"request_id": "r1"}))
+
+    assert control.stops == ["r1"]
+    assert control.submitted == []
+    assert control.called_now == []
+
+
+def test_get_state_is_the_one_service_run_on_the_loop():
+    control = SpyControl()
+
+    main._handle(RecordingNode(), control, input_event("request", row(service="get_state"), {"request_id": "r1"}))
+
+    assert control.called_now == [("r1", {"service": "get_state"})]
+
+
+def test_a_jog_goes_through_the_door_that_preempts():
+    # A jog drives the arm, so it takes the same road a motion does.
+    control = SpyControl()
+
+    main._handle(
+        RecordingNode(), control, input_event("jog", row(velocity=[1.0, 0, 0, 0, 0, 0]))
+    )
+
+    assert control.jogs == [[1.0, 0, 0, 0, 0, 0]]
+    assert control.submitted == []
+
+
+def test_a_jog_of_the_wrong_length_is_dropped_not_commanded():
+    control = SpyControl()
+
+    main._handle(RecordingNode(), control, input_event("jog", row(velocity=[1.0, 0])))
+
+    assert control.jogs == []
+
+
+def test_a_motion_goes_through_the_door_that_preempts():
+    # Still one protocol for the caller; the routing differs because the arm is
+    # one resource and a new motion replaces the old.
+    control = SpyControl()
+
+    main._handle(
+        RecordingNode(),
+        control,
+        input_event("request", row(service="move_to", x=1.0, y=2.0, z=3.0), {"request_id": "m1"}),
+    )
+
+    assert control.submitted == []
+    assert control.motions[0].request_id == "m1"
+    assert control.motions[0].wait is True
+
+
+def test_wait_false_is_read_off_the_request():
+    control = SpyControl()
+
+    main._handle(
+        RecordingNode(),
+        control,
+        input_event("request", row(service="home", wait=False), {"request_id": "m1"}),
+    )
+
+    assert control.motions[0].wait is False
+
+
+def test_a_malformed_request_is_refused_under_its_own_id_without_reaching_the_arm():
+    control = SpyControl()
+
+    main._handle(
+        RecordingNode(), control, input_event("request", row(service="teleport"), {"request_id": "r1"})
+    )
+
+    assert control.submitted == []
+    refusal = control.posted[0]
+    assert refusal.output == "response"
+    assert refusal.metadata == {"request_id": "r1"}
+    assert refusal.fields["ok"] is False
+    assert "teleport" in refusal.fields["message"]
+
+
+def test_an_unexpected_input_is_ignored():
+    node = handle(input_event("weather", row(sunny=True)))
+
+    assert node.sent == []
+
+
+def test_a_tick_publishes_a_response_under_its_request_id():
+    control = SpyControl()
+    control.post(Message("response", {"ok": True, "message": "done"}, {"request_id": "r1"}))
+
+    node = handle(input_event("tick", pa.array([0])), control)
+
+    assert node.outputs("response") == [({"ok": True, "message": "done"}, {"request_id": "r1"})]
+
+
+def test_a_tick_publishes_telemetry_and_state_changes():
+    control = SpyControl()
+    control._states = ["ready"]
+    control._telemetry = Telemetry([1.0] * 6, RobotPose(1, 2, 3), 42)
+
+    node = handle(input_event("tick", pa.array([0])), control)
+
+    sent = {name: (data, metadata) for name, data, metadata in node.sent}
+    assert sent["robot_state"][0].to_pylist() == ["ready"]
+    assert sent["joint_pose"][0].to_pylist() == [1.0] * 6
+    assert sent["tcp_pose"][0].to_pylist() == pytest.approx([1, 2, 3, 180, 0, 0])
+    assert sent["tcp_pose"][1]["frame"] == "tool_tip"
+
+
+def test_a_tick_with_nothing_new_publishes_nothing():
+    node = handle(input_event("tick", pa.array([0])))
+
+    assert node.sent == []
+
+
+def test_the_loop_runs_until_dora_stops_it(monkeypatch):
+    # The loop must drain on tick and leave on STOP, shutting the threads down.
+    control = SpyControl()
+    control.post(Message("response", {"ok": True}, {"request_id": "r1"}))
+
+    node = RecordingNode([input_event("tick", pa.array([0])), {"type": "STOP"}])
+    monkeypatch.setattr(main, "Node", lambda name: node)
+    monkeypatch.setattr(main, "Control", lambda config, restarted: control)
+    monkeypatch.setattr(main, "_parse_args", lambda: type("Args", (), {"name": "ulite6"})())
+    monkeypatch.setattr(Config, "from_env", classmethod(lambda cls: Config()))
+    control.start = lambda: None
+
+    main.main()
+
+    assert node.outputs("response")[0][1] == {"request_id": "r1"}
+    assert control.shutdowns == [True]

tests/test_preempt.py

@@ -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

tests/test_services.py

@@ -0,0 +1,275 @@
+"""Services: one request, one response, correlated by request_id."""
+
+import threading
+
+import pytest
+
+from dora_ulite6 import payload, services
+from dora_ulite6.config import Config
+from dora_ulite6.control import MOVING, READY, Control, ServiceCall
+from dora_ulite6.errors import RobotUnavailableError, UnreachablePoseError
+from tests.conftest import FakeArm, wait_for
+
+
+@pytest.fixture
+def control(monkeypatch):
+    """Start a Control over a fake arm, and shut it down when the test ends."""
+    arms = []
+
+    def build(config):
+        arm = FakeArm(config)
+        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
+    control.shutdown()
+
+
+def responses(control):
+    """Wait for at least one response and return them as (row, metadata) pairs."""
+    found = []
+
+    def got_one():
+        found.extend(m for m in control.take_messages() if m.output == "response")
+        return found
+
+    wait_for(got_one)
+    return [(message.fields, message.metadata) for message in found]
+
+
+def test_a_response_comes_back_under_the_requests_own_id(control):
+    control.submit(ServiceCall(request_id="req-7", row={"service": "clear"}))
+
+    fields, metadata = responses(control)[0]
+
+    assert metadata == {"request_id": "req-7"}
+    assert fields["ok"] is True
+    assert fields["service"] == "clear"
+    assert "clear" in control.arms[0].calls
+
+
+def test_get_state_answers_with_everything_a_caller_would_otherwise_poll(control):
+    control.submit(ServiceCall(request_id="s1", row={"service": "get_state"}))
+
+    fields, _ = responses(control)[0]
+
+    assert fields["ok"] is True
+    assert fields["connected"] is True
+    assert fields["ready"] is True
+    assert fields["pose"] == [300, 0, 250, 180.0, 0.0, 0.0]
+    assert fields["joints"] == [0.0] * 6
+
+
+def test_the_tool_offset_reaches_the_state_response(monkeypatch):
+    monkeypatch.setattr("dora_ulite6.control.ULite6", FakeArm)
+    control = Control(Config(telemetry_interval_s=0.01, tool_offset_mm=61.0))
+    control.start()
+    try:
+        wait_for(lambda: control.robot is not None)
+        control.submit(ServiceCall(request_id="s1", row={"service": "get_state"}))
+        fields, _ = responses(control)[0]
+    finally:
+        control.shutdown()
+
+    assert fields["pose"][2] == pytest.approx(250 - 61.0)
+
+
+def test_freedrive_reads_its_argument(control):
+    control.submit(ServiceCall(request_id="s1", row={"service": "freedrive", "enabled": False}))
+    responses(control)
+
+    assert ("set_freedrive", False) in control.arms[0].calls
+
+
+def test_grasp_and_release_go_through_the_mounted_tool(control):
+    control.submit(ServiceCall(request_id="s1", row={"service": "grasp"}))
+    responses(control)
+    control.submit(ServiceCall(request_id="s2", row={"service": "release"}))
+    responses(control)
+
+    assert "engage_tool" in control.arms[0].calls
+    assert "release_tool" in control.arms[0].calls
+
+
+def test_a_refusing_arm_makes_a_failed_response_not_an_exception(control):
+    control.arms[0].recover_error = RobotUnavailableError("still unreachable")
+    control.arms[0].command_error = RobotUnavailableError("cable pulled")
+
+    control.submit(ServiceCall(request_id="s1", row={"service": "freedrive"}))
+    fields, metadata = responses(control)[0]
+
+    assert fields["ok"] is False
+    assert "cable pulled" in fields["message"]
+    assert metadata == {"request_id": "s1"}
+
+
+def test_get_state_on_a_dead_link_says_so_rather_than_handing_back_a_stale_pose(control):
+    control.arms[0].read_error = RobotUnavailableError("cable pulled")
+
+    control.call_now("s1", {"service": "get_state"})
+    fields = control.take_messages()[0].fields
+
+    assert fields["connected"] is False
+    assert fields["pose"] == []
+
+
+def test_get_state_answers_during_a_motion_rather_than_queueing_behind_it(control):
+    # The whole point of polling: it must work while the arm is moving.
+    control.arms[0].block = threading.Event()
+    control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}, wait=False))
+    wait_for(lambda: "go_home" in control.arms[0].calls)
+
+    control.call_now("s1", {"service": "get_state"})
+
+    answers = [m for m in control.take_messages() if m.output == "response" and m.metadata == {"request_id": "s1"}]
+    assert answers and answers[0].fields["ok"] is True
+    control.arms[0].block.set()
+
+
+def test_stop_does_not_queue_behind_the_motion_it_aborts(control):
+    # The motion service is held open the way a real one holds the thread.
+    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)
+
+    # Queued, this would sit behind `home` until the motion ended.
+    control.request_stop("s1")
+
+    wait_for(lambda: "stop" in control.arms[0].calls)
+    assert any(metadata == {"request_id": "s1"} for _, metadata in responses(control))
+
+
+def test_stop_answers_without_waiting_for_the_arm_to_confirm(control):
+    # The SDK spends up to three seconds confirming a stop. The caller is
+    # answered at once and learns the outcome from the state, like everything
+    # else — the alternative is a dataflow that stops routing for three seconds.
+    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)
+
+    control.request_stop("s1")
+    answers = [m for m in control.take_messages() if m.metadata == {"request_id": "s1"}]
+
+    # Answered on this thread, before the stopper thread has run at all.
+    assert answers and answers[0].fields["message"] == "stop commanded"
+
+
+def test_a_motion_service_answers_only_once_the_arm_has_stopped(control):
+    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)
+
+    # The arm is still moving, so the caller has no answer yet.
+    assert [m for m in control.take_messages() if m.output == "response"] == []
+
+    control.arms[0].block.set()
+    fields, metadata = responses(control)[0]
+
+    assert metadata == {"request_id": "m1"}
+    assert fields["ok"] is True
+    assert fields["message"] == "the arm is home"
+
+
+def test_telemetry_keeps_flowing_while_a_motion_blocks(control):
+    # The reporter thread is not the one inside the motion, which is what lets a
+    # caller follow a service whose response will not arrive for seconds.
+    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 wait_for(control.take_telemetry) is not None
+
+    control.arms[0].block.set()
+
+
+def test_the_state_says_moving_while_a_motion_service_runs(control):
+    control.arms[0].block = threading.Event()
+
+    control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
+    wait_for(lambda: MOVING in control.take_state_changes())
+
+    control.arms[0].block.set()
+    wait_for(lambda: READY in control.take_state_changes())
+
+
+def test_a_move_request_is_commanded_in_the_flange_frame(monkeypatch):
+    monkeypatch.setattr("dora_ulite6.control.ULite6", FakeArm)
+    control = Control(Config(telemetry_interval_s=0.01, tool_offset_mm=61.0))
+    control.start()
+    try:
+        wait_for(lambda: control.robot is not None)
+        control.submit(
+            ServiceCall(request_id="m1", row={"service": "move_to", "x": 300, "y": 0, "z": 100})
+        )
+        responses(control)
+        move = next(call for call in control.robot.calls if call[0] == "move_to")
+    finally:
+        control.shutdown()
+
+    # The caller asked for the tool tip at z=100, so the flange goes to 161.
+    assert move[1][2] == pytest.approx(161.0)
+
+
+def test_an_unreachable_pose_fails_the_request_and_leaves_the_arm_ready(control):
+    control.arms[0].move_error = UnreachablePoseError("no joint solution")
+
+    control.submit(
+        ServiceCall(request_id="m1", row={"service": "move_to", "x": 9000, "y": 0, "z": 0})
+    )
+    fields, metadata = responses(control)[0]
+
+    assert fields["ok"] is False
+    assert "no joint solution" in fields["message"]
+    assert metadata == {"request_id": "m1"}
+    # The target was wrong, not the arm, so nothing tried to recover it.
+    assert "recover" not in control.arms[0].calls
+
+
+def test_a_motion_missing_an_argument_is_refused_without_moving_the_arm(control):
+    control.submit(ServiceCall(request_id="m1", row={"service": "move_to", "x": 1, "y": 2}))
+    fields, _ = responses(control)[0]
+
+    assert fields["ok"] is False
+    assert "missing `z`" in fields["message"]
+    assert control.arms[0].calls == []
+
+
+def test_move_joints_reads_its_angles(control):
+    control.submit(
+        ServiceCall(request_id="m1", row={"service": "move_joints", "angles": [1, 2, 3, 4, 5, 6]})
+    )
+    responses(control)
+
+    assert ("set_joint_angles", [1.0, 2.0, 3.0, 4.0, 5.0, 6.0], None) in control.arms[0].calls
+
+
+@pytest.mark.parametrize(
+    ("row", "expected"),
+    [
+        ({}, "unknown service"),
+        ({"service": "teleport"}, "unknown service"),
+        ({"service": 7}, "unknown service"),
+    ],
+)
+def test_a_row_naming_no_known_service_is_refused(row, expected):
+    with pytest.raises(payload.PayloadError, match=expected):
+        payload.name_of(row, services.SERVICE, services.SERVICES)
+
+
+def test_an_immediate_service_before_the_arm_is_reached_still_gets_an_answer(monkeypatch):
+    # `stop` runs on the event loop, so it can arrive before the control thread
+    # has built the driver. A caller waiting on a request_id must never hang.
+    monkeypatch.setattr("dora_ulite6.control.ULite6", FakeArm)
+    control = Control(Config())
+
+    control.call_now("s1", {"service": "stop"})
+
+    message = control.take_messages()[0]
+    assert message.metadata == {"request_id": "s1"}
+    assert message.fields["ok"] is False
+    assert "not been reached" in message.fields["message"]