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