cristhian
fdabf7e241
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.
165 lines
4.7 KiB
Python
165 lines
4.7 KiB
Python
"""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")
|