refactor node into services over the dora 1.0 contract

Replaces the command/status JSON pair with dora's own service pattern:
one request, one response, correlated by the request_id dora puts in the
message metadata. Motions take a wait flag, and their lifecycle is
published as the arm's state on motion_state rather than as a per-goal
stream — the arm is one resource, so a new motion preempts the one in
play and a state describes that exactly.

motion_state is keyed by request_id and publishes accepted before the
control thread reaches the motion. Both exist to close one race: a caller
polling a bare moving/idle flag right after its request would read "not
moving" and conclude the motion had already finished.

Three threads, because the SDK blocks. The control thread carries out one
job at a time and may block; the reporter thread only ever reads the
report cache, so telemetry keeps flowing through a ten-second move; the
stopper thread exists because a stop fits nowhere else — it must not
queue behind the motion it aborts, and emergency_stop re-commands the
controller for up to three seconds, which is far too long to stop routing
a dataflow.

The event loop never calls anything that can block, get_state included:
that one is allowed there only because it reads the cache. The node is
built before the arm threads start, so INIT_MODE cannot move a robot
before the process has joined the dataflow, and a restart leaves the arm
where it is rather than driving it home unasked. Shutdown halts the arm
first — a motion lives in the controller, and dropping the connection
does not abort it.
This commit is contained in:
2026-09-12 11:46:59 -03:00
parent eccd9e3c24
commit fdabf7e241
11 changed files with 2089 additions and 1024 deletions
+164
View File
@@ -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")