diff --git a/dora_ulite6/__init__.py b/dora_ulite6/__init__.py
index 1dad1aa..38bd87f 100644
--- a/dora_ulite6/__init__.py
+++ b/dora_ulite6/__init__.py
@@ -1 +1 @@
-"""Dora ULite6 node package."""
+"""Dora node for a UFactory Lite6 arm: telemetry out, commands in."""
diff --git a/dora_ulite6/control.py b/dora_ulite6/control.py
new file mode 100644
index 0000000..ba45db7
--- /dev/null
+++ b/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())
+
diff --git a/dora_ulite6/main.py b/dora_ulite6/main.py
index c90ad41..a05c6b0 100644
--- a/dora_ulite6/main.py
+++ b/dora_ulite6/main.py
@@ -1,1049 +1,178 @@
-"""Dora node for controlling a UFactory Lite6 robot and publishing state."""
+"""The dora event loop: accept requests, publish what the arm threads produced.
-import json
+Nothing here touches the arm. The loop reads the `request_id` dora puts in a
+message's metadata, hands the work to the control thread, and publishes the
+answer under the same id.
+
+Two services do not take the ordinary road. `get_state` runs here, because it
+never blocks and is wanted exactly when the control thread is busy. `stop` is
+handed to the stopper thread, because it must not queue behind the motion it
+aborts and must not be run here either — the SDK spends up to three seconds on
+it, and a router cannot go away that long.
+"""
+
+import argparse
import os
-import threading
-import time
-from typing import Any, Dict, Optional
-import numpy as np
import pyarrow as pa
-import uvicorn
from dora import Node
-from fastapi import FastAPI, HTTPException
-from fastapi.responses import HTMLResponse
-from pydantic import BaseModel
-from scipy.spatial.transform import Rotation
-from xarm.wrapper import XArmAPI
+from dora_ulite6 import payload, services
+from dora_ulite6.config import Config
+from dora_ulite6.control import Control, Message, ServiceCall, Telemetry
+from dora_ulite6.log import log
-class ULite6Helper:
- """Minimal ULite6 helper based on ufactory-control."""
-
- def __init__(self, robot_ip: str):
- self.robot_ip = robot_ip
- self.arm = XArmAPI(self.robot_ip)
- self.arm.connect()
- self.arm.motion_enable(enable=True)
-
- def go_home(self) -> int:
- self.arm.set_mode(0)
- self.arm.set_state(state=0)
- code = self.arm.move_gohome(wait=True)
- # move_gohome may return tuple (code, result) in some versions
- if isinstance(code, tuple):
- code = code[0]
- return code
-
- def move_to_pose(
- self,
- x: float,
- y: float,
- z: float,
- roll: float,
- pitch: float,
- yaw: float,
- speed: float = 30.0,
- units: str = "mm",
- ) -> int:
- if units == "m":
- x, y, z = x * 1000.0, y * 1000.0, z * 1000.0
- self.arm.set_mode(0)
- self.arm.set_state(0)
- return self.arm.set_position(
- x, y, z, roll=roll, pitch=pitch, yaw=yaw, speed=speed, wait=True
- )
-
- def get_current_position(self) -> Dict[str, float]:
- code, pos = self.arm.get_position()
- if code != 0:
- raise RuntimeError(f"get_position failed with code {code}")
- return {
- "x": pos[0],
- "y": pos[1],
- "z": pos[2],
- "roll": pos[3],
- "pitch": pos[4],
- "yaw": pos[5],
- }
-
- def get_tool_tcp_pose(self, offset_mm: float) -> Dict[str, float]:
- pos = self.get_current_position()
- if offset_mm == 0.0:
- return pos
- T = self.get_pose_matrix(tcp_offset_mm=offset_mm)
- return {
- "x": T[0, 3] * 1000.0,
- "y": T[1, 3] * 1000.0,
- "z": T[2, 3] * 1000.0,
- "roll": pos["roll"],
- "pitch": pos["pitch"],
- "yaw": pos["yaw"],
- }
-
- def get_pose_matrix(self, tcp_offset_mm: float = 0.0) -> np.ndarray:
- pos = self.get_current_position()
- R = Rotation.from_euler(
- "xyz", [pos["roll"], pos["pitch"], pos["yaw"]], degrees=True
- ).as_matrix()
-
- position_m = np.array([pos["x"], pos["y"], pos["z"]]) / 1000.0
- if tcp_offset_mm != 0.0:
- tcp_offset_local = np.array([0, 0, tcp_offset_mm]) / 1000.0
- tcp_offset_global = R @ tcp_offset_local
- position_m = position_m + tcp_offset_global
-
- T = np.eye(4)
- T[:3, :3] = R
- T[:3, 3] = position_m
- return T
-
- def get_joint_angles_deg(self) -> np.ndarray:
- code, angles = self.arm.get_servo_angle(is_radian=False)
- if code != 0:
- raise RuntimeError(f"get_servo_angle failed with code {code}")
- return np.array(angles, dtype=float)
-
- def get_status(self) -> Dict[str, Any]:
- """Get robot status information."""
- return {
- "connected": self.arm.connected,
- "state": self.arm.state,
- "mode": self.arm.mode,
- "error_code": self.arm.error_code,
- "warn_code": self.arm.warn_code,
- "has_error": self.arm.has_error,
- "has_warn": self.arm.has_warn,
- "motor_enable_states": self.arm.motor_enable_states,
- }
-
- def reset_state(self) -> int:
- """Clear errors/warnings and reset robot state."""
- self.arm.clean_error()
- self.arm.clean_warn()
- self.arm.motion_enable(enable=True)
- self.arm.set_mode(0)
- return self.arm.set_state(0)
-
- def set_vacuum_gripper(self, on: bool) -> int:
- """Turn vacuum gripper on or off."""
- code = self.arm.set_vacuum_gripper(on)
- if isinstance(code, tuple):
- code = code[0]
- return code
-
- def get_vacuum_gripper(self) -> Dict[str, Any]:
- """Get vacuum gripper status."""
- code, status = self.arm.get_vacuum_gripper()
- return {"code": code, "on": status == 1}
-
- def disconnect(self) -> int:
- return self.arm.disconnect()
-
-
-# --- Pydantic models for API requests ---
-
-
-class MoveToRequest(BaseModel):
- x: float
- y: float
- z: float
- speed: Optional[float] = None
- roll: Optional[float] = 180.0
- pitch: Optional[float] = 0.0
- yaw: Optional[float] = 0.0
-
-
-class MoveToPoseRequest(BaseModel):
- x: float
- y: float
- z: float
- roll: float
- pitch: float
- yaw: float
- speed: Optional[float] = None
-
-
-# --- FastAPI app factory ---
-
-
-def create_api(
- helper: ULite6Helper,
- default_speed: float,
- default_units: str,
- vacuum_enabled: bool,
- tool_type: str,
- tool_offset_mm: float,
-) -> FastAPI:
- """Create FastAPI application with robot control endpoints."""
- app = FastAPI(
- title="ULite6 Robot Control API",
- description="REST API for controlling UFactory Lite6 robot",
- version="0.1.0",
- )
-
- @app.get("/", response_class=HTMLResponse)
- def web_ui():
- """Serve web control interface."""
- return """
-
-
-
-
- ULite6 Control
-
-
-
-
ULite6 Robot Control
-
-
-
Status
-
-
-
Connected
-
--
-
-
-
State
-
--
-
-
-
Mode
-
--
-
-
-
Error
-
--
-
-
-
Warning
-
--
-
-
-
-
-
-
TCP Pose (mm, deg)
-
-
X
--
-
Y
--
-
Z
--
-
Roll
--
-
Pitch
--
-
Yaw
--
-
-
Joint Angles (deg)
-
-
J1
--
-
J2
--
-
J3
--
-
J4
--
-
J5
--
-
J6
--
-
-
-
-
-
Controls
-
-
-
-
-
-
Vacuum Gripper
-
-
-
- Status: --
-
-
-
Move To Position
-
-
-
-
-
Response Log
-
-
-
-
-
-
-"""
-
- @app.get("/api/status")
- def get_status():
- """Get robot status (connected, enabled, errors)."""
- try:
- return helper.get_status()
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.get("/api/pose")
- def get_pose():
- """Get current TCP pose [x, y, z, roll, pitch, yaw] with tool offset applied."""
- try:
- return helper.get_tool_tcp_pose(tool_offset_mm)
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.get("/api/joints")
- def get_joints():
- """Get current joint angles in degrees."""
- try:
- angles = helper.get_joint_angles_deg()
- return {"joints": angles.tolist(), "units": "deg"}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/home")
- def go_home():
- """Send robot to home position."""
- try:
- code = helper.go_home()
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/move_to")
- def move_to(request: MoveToRequest):
- """Move to position with optional orientation."""
- try:
- speed = request.speed if request.speed is not None else default_speed
- code = helper.move_to_pose(
- request.x,
- request.y,
- request.z,
- request.roll,
- request.pitch,
- request.yaw,
- speed,
- default_units,
- )
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/move_to_pose")
- def move_to_pose(request: MoveToPoseRequest):
- """Move to full pose with position and orientation."""
- try:
- speed = request.speed if request.speed is not None else default_speed
- code = helper.move_to_pose(
- request.x,
- request.y,
- request.z,
- request.roll,
- request.pitch,
- request.yaw,
- speed,
- default_units,
- )
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/reset")
- def reset_state():
- """Clear errors/warnings and reset robot state."""
- try:
- code = helper.reset_state()
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/disconnect")
- def disconnect():
- """Disconnect from robot."""
- try:
- code = helper.disconnect()
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- # Vacuum gripper endpoints (only if enabled)
- if vacuum_enabled:
-
- @app.get("/api/vacuum")
- def get_vacuum():
- """Get vacuum gripper status."""
- try:
- return helper.get_vacuum_gripper()
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/vacuum/on")
- def vacuum_on():
- """Turn vacuum gripper on."""
- try:
- code = helper.set_vacuum_gripper(True)
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.post("/api/vacuum/off")
- def vacuum_off():
- """Turn vacuum gripper off."""
- try:
- code = helper.set_vacuum_gripper(False)
- return {"ok": code == 0, "code": code}
- except Exception as e:
- raise HTTPException(status_code=500, detail=str(e))
-
- @app.get("/api/config")
- def get_config():
- """Get API configuration."""
- return {
- "vacuum_enabled": vacuum_enabled,
- "tool_type": tool_type,
- "tool_offset_mm": tool_offset_mm,
- }
-
- return app
-
-
-def run_uvicorn(app: FastAPI, host: str, port: int) -> None:
- """Run uvicorn server (for use in background thread)."""
- config = uvicorn.Config(app, host=host, port=port, log_level="warning")
- server = uvicorn.Server(config)
- server.run()
-
-
-def _send_joint_pose(node: Node, joints_deg: np.ndarray) -> None:
- metadata = {"units": "deg", "timestamp_ns": time.time_ns()}
- node.send_output("joint_pose", pa.array(joints_deg.tolist()), metadata=metadata)
-
-
-def _send_tcp_pose(node: Node, tcp_pose: Dict[str, float]) -> None:
- metadata = {
- "units_position": "mm",
- "units_rotation": "deg",
- "timestamp_ns": time.time_ns(),
- }
- vec = [
- tcp_pose["x"],
- tcp_pose["y"],
- tcp_pose["z"],
- tcp_pose["roll"],
- tcp_pose["pitch"],
- tcp_pose["yaw"],
- ]
- node.send_output("tcp_pose", pa.array(vec), metadata=metadata)
-
-
-def _send_error_status(
- node: Node, action: str, message: str, status: Optional[Dict[str, Any]] = None
-) -> None:
- payload = {
- "ok": False,
- "action": action,
- "message": message,
- "timestamp_ns": time.time_ns(),
- }
- if status:
- payload.update(status)
- payload["type"] = "error"
- node.send_output("status", pa.array([json.dumps(payload)]))
-
-
-def _send_command_status(
- node: Node,
- command_id: str,
- action: str,
- ok: bool,
- message: str,
- state: str,
- code: Optional[int] = None,
- status: Optional[Dict[str, Any]] = None,
-) -> None:
- payload = {
- "ok": ok,
- "action": action,
- "message": message,
- "timestamp_ns": time.time_ns(),
- "command_id": command_id,
- }
- if code is not None:
- payload["code"] = code
- if status:
- payload.update(status)
- payload["type"] = "command"
- payload["state"] = state
- node.send_output("status", pa.array([json.dumps(payload)]))
-
-
-def _status_snapshot(helper: ULite6Helper) -> Dict[str, Any]:
- status = helper.get_status()
- return {
- "robot_state": status.get("state"),
- "mode": status.get("mode"),
- "error_code": status.get("error_code"),
- "warn_code": status.get("warn_code"),
- "has_error": status.get("has_error"),
- "has_warn": status.get("has_warn"),
- }
-
-
-def _log(msg: str) -> None:
- """Print a timestamped log message."""
- timestamp = time.strftime("%H:%M:%S")
- print(f"[ulite6 {timestamp}] {msg}", flush=True)
+# The metadata key dora's service pattern is built on.
+REQUEST_ID = "request_id"
def main() -> None:
- node = Node()
+ """Connect to the dataflow, start the arm threads, and run until dora stops us."""
+ args = _parse_args()
+ config = Config.from_env()
- robot_ip = os.getenv("ROBOT_IP", "192.168.1.192")
- default_speed = float(os.getenv("DEFAULT_SPEED", "30"))
- default_units = os.getenv("DEFAULT_UNITS", "mm")
- api_host = os.getenv("API_HOST", "0.0.0.0")
- api_port = int(os.getenv("API_PORT", "8080"))
- vacuum_enabled = os.getenv("VACUUM_ENABLED", "false").lower() in ("true", "1", "yes")
+ # The node comes first, and the arm afterwards. Starting the arm threads
+ # first would let INIT_MODE drive a real robot before this process had
+ # joined the dataflow at all — and if joining then failed, it would have
+ # moved with nobody watching. Connecting first also lets the arm be told
+ # whether this is a restart.
+ #
+ # Passing the name lets the node also run standalone as a dynamic node; when
+ # dora spawns it, the id from the dataflow wins.
+ node = Node(args.name)
- _TOOL_OFFSET_PRESETS: Dict[str, float] = {"none": 0.0, "vacuum": 61.0, "gripper": 65.0}
- tool_type = os.getenv("TOOL_TYPE", "none").lower()
- tool_offset_mm = _TOOL_OFFSET_PRESETS.get(tool_type, 0.0) + float(os.getenv("TOOL_OFFSET_MM", "0.0"))
+ control = Control(config, restarted=node.is_restart())
+ control.start()
- # Initial position settings
- init_mode = os.getenv("INIT_MODE", "none").lower() # "home", "pose", or "none"
- init_x = float(os.getenv("INIT_X", "300.0"))
- init_y = float(os.getenv("INIT_Y", "0.0"))
- init_z = float(os.getenv("INIT_Z", "250.0"))
- init_roll = float(os.getenv("INIT_ROLL", "180.0"))
- init_pitch = float(os.getenv("INIT_PITCH", "0.0"))
- init_yaw = float(os.getenv("INIT_YAW", "0.0"))
- init_speed = float(os.getenv("INIT_SPEED", "50.0"))
+ try:
+ for event in node:
+ if event["type"] == "ERROR":
+ raise RuntimeError(event["error"])
+ if event["type"] != "INPUT":
+ continue
+ _handle(node, control, event)
+ finally:
+ if not control.shutdown():
+ # The arm has been told to stop, but the control thread is still
+ # inside a blocking SDK call and may never come out. Leave rather
+ # than hang until dora loses patience and kills us.
+ os._exit(0)
- _log(f"Connecting to robot at {robot_ip}...")
- helper = ULite6Helper(robot_ip)
- _log("Robot connected")
- # Move to initial position on startup
- if init_mode == "home":
- _log("Moving to home position...")
- code = helper.go_home()
- if code == 0:
- _log("Home position reached")
- else:
- _log(f"Home failed with code {code}")
- elif init_mode == "pose":
- _log(f"Moving to initial pose: [{init_x}, {init_y}, {init_z}] roll={init_roll} pitch={init_pitch} yaw={init_yaw}")
- code = helper.move_to_pose(
- init_x, init_y, init_z,
- init_roll, init_pitch, init_yaw,
- speed=init_speed,
- units="mm",
- )
- if code == 0:
- _log("Initial pose reached")
- else:
- _log(f"Move to initial pose failed with code {code}")
+def _handle(node: Node, control: Control, event: dict) -> None:
+ """Route one input event."""
+ event_id = event["id"]
+ if event_id == "tick":
+ _publish(node, control)
+ elif event_id == "request":
+ _accept_request(control, event)
+ elif event_id == "jog":
+ _accept_jog(control, event)
else:
- _log("Skipping initial position (INIT_MODE=none)")
+ log(f"ignoring unexpected input `{event_id}`")
- # Create and start FastAPI server in background thread
- app = create_api(helper, default_speed, default_units, vacuum_enabled, tool_type, tool_offset_mm)
- api_thread = threading.Thread(
- target=run_uvicorn, args=(app, api_host, api_port), daemon=True
+
+def _publish(node: Node, control: Control) -> None:
+ """Send everything the arm threads produced since the last tick."""
+ for state in control.take_state_changes():
+ node.send_output("robot_state", pa.array([state]))
+ for message in control.take_messages():
+ node.send_output(message.output, pa.array([message.fields]), metadata=message.metadata)
+ telemetry = control.take_telemetry()
+ if telemetry is not None:
+ _send_telemetry(node, telemetry)
+
+
+def _send_telemetry(node: Node, telemetry: Telemetry) -> None:
+ node.send_output(
+ "joint_pose",
+ pa.array(telemetry.joints, type=pa.float32()),
+ metadata={"units": "deg", "timestamp_ns": telemetry.timestamp_ns},
+ )
+ node.send_output(
+ "tcp_pose",
+ pa.array(telemetry.tip.as_list(), type=pa.float32()),
+ metadata={
+ "units_position": "mm",
+ "units_rotation": "deg",
+ "frame": "tool_tip",
+ "timestamp_ns": telemetry.timestamp_ns,
+ },
)
- api_thread.start()
- _log(f"Web interface at http://{api_host}:{api_port}")
- # Dora event loop - only handles tick for state publishing
- for event in node:
- if event["type"] != "INPUT":
- continue
- if event["id"] == "tick":
- try:
- joints = helper.get_joint_angles_deg()
- tcp_pose = helper.get_tool_tcp_pose(tool_offset_mm)
- _send_joint_pose(node, joints)
- _send_tcp_pose(node, tcp_pose)
- except Exception as exc:
- _send_error_status(
- node, "publish_state", str(exc), status=_status_snapshot(helper)
- )
- elif event["id"] == "command":
- try:
- value = event["value"]
- raw = value[0].as_py() if len(value) else None
- if not raw:
- _send_command_status(
- node,
- command_id="",
- action="command",
- ok=False,
- message="Empty command payload",
- state="failed",
- status=_status_snapshot(helper),
- )
- continue
- payload = json.loads(raw)
- except Exception as exc:
- _send_command_status(
- node,
- command_id="",
- action="command",
- ok=False,
- message=f"Invalid command payload: {exc}",
- state="failed",
- status=_status_snapshot(helper),
- )
- continue
+def _accept_request(control: Control, event: dict) -> None:
+ """Take one service request, or refuse it under its own request id.
- command_id = str(payload.get("id", ""))
- action = str(payload.get("action", ""))
- data = payload.get("payload", {}) if isinstance(payload.get("payload", {}), dict) else {}
+ A request the loop cannot parse is answered here rather than queued: a
+ caller waiting on a request_id must get exactly one response, never silence.
+ A request it *can* parse is queued even before the arm has been reached,
+ because `recover` and `get_state` are precisely what a caller sends then.
- if not command_id:
- _send_command_status(
- node,
- command_id="",
- action=action or "command",
- ok=False,
- message="Missing command id",
- state="failed",
- status=_status_snapshot(helper),
- )
- continue
+ `wait` decides only *when* the one response is sent — at once, or when the
+ arm stops. It is read here rather than in the service handler because it is
+ about the protocol, not about what the arm does.
+ """
+ request_id = event["metadata"].get(REQUEST_ID, "")
+ try:
+ row = payload.read_row(event["value"])
+ name = payload.name_of(row, services.SERVICE, services.NAMES)
+ wait = payload.flag(row, "wait", True)
+ except payload.PayloadError as error:
+ _refuse(control, request_id, str(error))
+ return
+ if name == services.STOP:
+ control.request_stop(request_id)
+ return
+ if name in services.IMMEDIATE:
+ control.call_now(request_id, row)
+ return
+ call = ServiceCall(request_id=request_id, row=row, wait=wait)
+ if call.moves_the_arm:
+ # Motions go through their own door: one preempts whatever is already
+ # moving, rather than queueing behind it.
+ control.start_motion(call)
+ return
+ control.submit(call)
- try:
- if action == "home":
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=True,
- message="Executing home",
- state="started",
- )
- code = helper.go_home()
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=code == 0,
- message="Home command executed",
- state="done" if code == 0 else "failed",
- code=code,
- status=_status_snapshot(helper),
- )
- elif action in ("move_to", "move_to_pose"):
- speed = float(data.get("speed", default_speed))
- x = float(data.get("x", 0.0))
- y = float(data.get("y", 0.0))
- z = float(data.get("z", 0.0))
- roll = float(data.get("roll", 180.0))
- pitch = float(data.get("pitch", 0.0))
- yaw = float(data.get("yaw", 0.0))
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=True,
- message="Executing move",
- state="started",
- )
- code = helper.move_to_pose(
- x,
- y,
- z,
- roll,
- pitch,
- yaw,
- speed,
- default_units,
- )
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=code == 0,
- message="Move command executed",
- state="done" if code == 0 else "failed",
- code=code,
- status=_status_snapshot(helper),
- )
- elif action in ("vacuum_on", "vacuum_off"):
- if not vacuum_enabled:
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=False,
- message="Vacuum gripper not enabled",
- state="failed",
- status=_status_snapshot(helper),
- )
- continue
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=True,
- message="Executing vacuum",
- state="started",
- )
- vacuum_on = action == "vacuum_on"
- code = helper.set_vacuum_gripper(vacuum_on)
- _send_command_status(
- node,
- command_id=command_id,
- action=action,
- ok=code == 0,
- message="Vacuum command executed",
- state="done" if code == 0 else "failed",
- code=code,
- status=_status_snapshot(helper),
- )
- else:
- _send_command_status(
- node,
- command_id=command_id,
- action=action or "command",
- ok=False,
- message=f"Unknown action: {action}",
- state="failed",
- status=_status_snapshot(helper),
- )
- except Exception as exc:
- _send_command_status(
- node,
- command_id=command_id,
- action=action or "command",
- ok=False,
- message=str(exc),
- state="failed",
- status=_status_snapshot(helper),
- )
+
+def _accept_jog(control: Control, event: dict) -> None:
+ """Take one velocity command.
+
+ A topic rather than a service: a jog is a stream that expires on the
+ controller's own TTL, so a sender keeps sending and nobody waits for an
+ answer. It still drives the arm, so it preempts a motion and reports
+ `moving` the same way one does — an arm that is travelling must never look
+ idle.
+ """
+ try:
+ row = payload.read_row(event["value"])
+ velocity = payload.numbers(row, "velocity", count=6)
+ except payload.PayloadError as error:
+ log(f"ignoring a malformed jog: {error}")
+ return
+ control.start_jog(velocity)
+
+
+def _refuse(control: Control, request_id: str, message: str) -> None:
+ """Answer a request the loop rejected, without troubling the arm threads."""
+ control.post(
+ Message(
+ output="response",
+ fields={"ok": False, "service": "", "message": message},
+ metadata={REQUEST_ID: request_id},
+ )
+ )
+
+
+def _parse_args() -> argparse.Namespace:
+ parser = argparse.ArgumentParser(description="Dora node for a UFactory Lite6 arm.")
+ parser.add_argument(
+ "--name",
+ default="ulite6",
+ help="The name of the node in the dataflow, used when running as a dynamic node.",
+ )
+ return parser.parse_args()
if __name__ == "__main__":
diff --git a/dora_ulite6/payload.py b/dora_ulite6/payload.py
new file mode 100644
index 0000000..90ecd0b
--- /dev/null
+++ b/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),
+ )
diff --git a/dora_ulite6/services.py b/dora_ulite6/services.py
new file mode 100644
index 0000000..b1746a1
--- /dev/null
+++ b/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}
diff --git a/tests/__init__.py b/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/conftest.py b/tests/conftest.py
new file mode 100644
index 0000000..2517f0e
--- /dev/null
+++ b/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")
diff --git a/tests/test_lifecycle.py b/tests/test_lifecycle.py
new file mode 100644
index 0000000..044b585
--- /dev/null
+++ b/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"
diff --git a/tests/test_loop.py b/tests/test_loop.py
new file mode 100644
index 0000000..8a8fb07
--- /dev/null
+++ b/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]
diff --git a/tests/test_preempt.py b/tests/test_preempt.py
new file mode 100644
index 0000000..adc02df
--- /dev/null
+++ b/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
diff --git a/tests/test_services.py b/tests/test_services.py
new file mode 100644
index 0000000..ff8e5f9
--- /dev/null
+++ b/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"]