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