From fdabf7e241d76c5898936dc5dc291d24b74fe19c Mon Sep 17 00:00:00 2001 From: Cristhian Date: Sat, 12 Sep 2026 11:46:59 -0300 Subject: [PATCH] refactor node into services over the dora 1.0 contract MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replaces the command/status JSON pair with dora's own service pattern: one request, one response, correlated by the request_id dora puts in the message metadata. Motions take a wait flag, and their lifecycle is published as the arm's state on motion_state rather than as a per-goal stream — the arm is one resource, so a new motion preempts the one in play and a state describes that exactly. motion_state is keyed by request_id and publishes accepted before the control thread reaches the motion. Both exist to close one race: a caller polling a bare moving/idle flag right after its request would read "not moving" and conclude the motion had already finished. Three threads, because the SDK blocks. The control thread carries out one job at a time and may block; the reporter thread only ever reads the report cache, so telemetry keeps flowing through a ten-second move; the stopper thread exists because a stop fits nowhere else — it must not queue behind the motion it aborts, and emergency_stop re-commands the controller for up to three seconds, which is far too long to stop routing a dataflow. The event loop never calls anything that can block, get_state included: that one is allowed there only because it reads the cache. The node is built before the arm threads start, so INIT_MODE cannot move a robot before the process has joined the dataflow, and a restart leaves the arm where it is rather than driving it home unasked. Shutdown halts the arm first — a motion lives in the controller, and dropping the connection does not abort it. --- dora_ulite6/__init__.py | 2 +- dora_ulite6/control.py | 552 ++++++++++++++++++ dora_ulite6/main.py | 1175 +++++---------------------------------- dora_ulite6/payload.py | 91 +++ dora_ulite6/services.py | 167 ++++++ tests/__init__.py | 0 tests/conftest.py | 164 ++++++ tests/test_lifecycle.py | 241 ++++++++ tests/test_loop.py | 215 +++++++ tests/test_preempt.py | 231 ++++++++ tests/test_services.py | 275 +++++++++ 11 files changed, 2089 insertions(+), 1024 deletions(-) create mode 100644 dora_ulite6/control.py create mode 100644 dora_ulite6/payload.py create mode 100644 dora_ulite6/services.py create mode 100644 tests/__init__.py create mode 100644 tests/conftest.py create mode 100644 tests/test_lifecycle.py create mode 100644 tests/test_loop.py create mode 100644 tests/test_preempt.py create mode 100644 tests/test_services.py 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

-
- - -
- -

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"]