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.
216 lines
6.5 KiB
Python
216 lines
6.5 KiB
Python
"""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]
|