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.
276 lines
9.9 KiB
Python
276 lines
9.9 KiB
Python
"""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"]
|