"""Starting up, jogging and shutting down: the node's behaviour as a dora node.""" import threading import time import pytest from dora_ulite6.config import Config, MotionConfig from dora_ulite6.control import MOVING, PREEMPTED, READY, Control, ServiceCall from dora_ulite6.pose import RobotPose from tests.conftest import FakeArm, wait_for def build(monkeypatch, **config): """Return a started Control over a fake arm, and the arms it built.""" arms = [] def make(cfg): arm = FakeArm(cfg) arms.append(arm) return arm monkeypatch.setattr("dora_ulite6.control.ULite6", make) restarted = config.pop("restarted", False) control = Control(Config(telemetry_interval_s=0.01, **config), restarted=restarted) control.arms = arms control.start() wait_for(lambda: control.robot is not None) return control # --- shutting down --- def test_shutdown_halts_the_arm_before_leaving(monkeypatch): # A motion lives in the controller, not in this process: exiting without # commanding a stop leaves an arm travelling with nobody left to stop it. control = build(monkeypatch) 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 control.shutdown() is True assert "stop" in control.arms[0].calls def test_shutdown_reports_a_control_thread_it_could_not_get_back(monkeypatch): # The caller's cue to leave rather than hang until dora kills the process. control = build(monkeypatch, motion=MotionConfig(ready_timeout_s=0.1)) control.arms[0].block = threading.Event() # An arm that ignores the stop is one whose thread will not come back. control.arms[0].stop = lambda: control.arms[0].calls.append("stop") control.submit(ServiceCall(request_id="m1", row={"service": "home"})) wait_for(lambda: "go_home" in control.arms[0].calls) assert control.shutdown() is False control.arms[0].block.set() def test_shutdown_is_clean_when_the_arm_was_idle(monkeypatch): control = build(monkeypatch) assert control.shutdown() is True # --- restarting --- @pytest.mark.parametrize("init_mode", ["home", "pose"]) def test_a_restart_leaves_the_arm_where_it_is(monkeypatch, init_mode): # Driving a real arm to INIT_MODE because the process came back is a motion # nobody asked for. control = build(monkeypatch, init_mode=init_mode, restarted=True) try: wait_for(lambda: READY in control.take_state_changes()) assert control.arms[0].calls == [] finally: control.shutdown() def test_a_first_start_still_runs_init_mode(monkeypatch): control = build(monkeypatch, init_mode="home") try: wait_for(lambda: "go_home" in control.arms[0].calls) finally: control.shutdown() # --- jogging --- def test_a_jog_makes_the_arm_report_that_it_is_moving(monkeypatch): # An arm that is travelling must never look idle. control = build(monkeypatch) try: wait_for(lambda: READY in control.take_state_changes()) control.start_jog([20.0, 0, 0, 0, 0, 0]) assert MOVING in control.take_state_changes() wait_for(lambda: any(c[0] == "jog" for c in control.arms[0].calls if isinstance(c, tuple))) finally: control.shutdown() def test_a_stream_of_jogs_is_one_state_change_not_hundreds(monkeypatch): control = build(monkeypatch) try: wait_for(lambda: READY in control.take_state_changes()) for _ in range(20): control.start_jog([20.0, 0, 0, 0, 0, 0]) assert control.take_state_changes() == [MOVING] finally: control.shutdown() def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch): control = build(monkeypatch) try: wait_for(lambda: READY in control.take_state_changes()) control.start_jog([20.0, 0, 0, 0, 0, 0]) control.take_state_changes() control.start_jog([0.0] * 6) assert READY in control.take_state_changes() finally: control.shutdown() def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatch): # A teleop source streaming zero velocity says nothing about a motion that # owns the arm, and must not answer for it: an arm that is travelling must # never look idle, and a `ready` here would also stop the motion's own end # from ever being published. control = build(monkeypatch) control.arms[0].block = threading.Event() try: wait_for(lambda: READY in control.take_state_changes()) control.submit(ServiceCall(request_id="m1", row={"service": "home"})) wait_for(lambda: MOVING in control.take_state_changes()) for _ in range(5): control.start_jog([0.0] * 6) assert control.take_state_changes() == [] control.arms[0].block.set() wait_for(lambda: READY in control.take_state_changes()) finally: control.shutdown() def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch): # The controller expires the command on its own; nothing tells us, so the # clock has to. control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02)) try: wait_for(lambda: READY in control.take_state_changes()) control.start_jog([20.0, 0, 0, 0, 0, 0]) control.take_state_changes() wait_for(lambda: READY in control.take_state_changes()) finally: control.shutdown() def test_a_jog_preempts_a_motion_that_is_running(monkeypatch): control = build(monkeypatch) control.arms[0].block = threading.Event() try: control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}, wait=False)) wait_for(lambda: "go_home" in control.arms[0].calls) control.start_jog([20.0, 0, 0, 0, 0, 0]) wait_for(lambda: "stop" in control.arms[0].calls) states = _motion_states(control, "m1") wait_for(lambda: PREEMPTED in _motion_states(control, "m1", states)) finally: control.arms[0].block.set() control.shutdown() def test_a_motion_leaves_velocity_mode_through_the_front_door(monkeypatch): # A pose command switches the controller out of velocity mode behind the # driver's back, leaving its own bookkeeping wrong. control = build(monkeypatch) try: wait_for(lambda: READY in control.take_state_changes()) control.start_jog([20.0, 0, 0, 0, 0, 0]) wait_for(lambda: any(isinstance(c, tuple) and c[0] == "jog" for c in control.arms[0].calls)) control.start_motion( ServiceCall(request_id="m1", row={"service": "move_to", "x": 1.0, "y": 2.0, "z": 3.0}, wait=False) ) wait_for(lambda: any(isinstance(c, tuple) and c[0] == "move_to" for c in control.arms[0].calls)) jogs = [c for c in control.arms[0].calls if isinstance(c, tuple) and c[0] == "jog"] assert jogs[-1] == ("jog", [0.0] * 6) finally: control.shutdown() def _motion_states(control, request_id, into=None): collected = into if into is not None else [] collected.extend( message.fields["state"] for message in control.take_messages() if message.output == "motion_state" and message.fields["request_id"] == request_id ) return collected # --- the report invariant get_state rests on --- def test_get_state_reads_the_cache_not_the_wire(monkeypatch): control = build(monkeypatch) try: control.arms[0].pose = RobotPose(1, 2, 3) control.call_now("s1", {"service": "get_state"}) fields = control.take_messages()[0].fields assert fields["pose"][:3] == [1, 2, 3] # Nothing that costs a round trip was called. assert control.arms[0].calls == [] finally: control.shutdown() # --- the cadence itself --- def test_the_reporter_honours_the_telemetry_interval(monkeypatch): """The interval is a real sleep, not a flag that makes every wait return at once. Worth a test of its own because the failure is silent: the node behaves correctly in every other respect while burning a core and reading the arm a million times a second. """ reads = [] class Counting(FakeArm): def read_report(self): reads.append(1) return super().read_report() monkeypatch.setattr("dora_ulite6.control.ULite6", Counting) control = Control(Config(telemetry_interval_s=0.05)) control.start() try: wait_for(lambda: reads) reads.clear() time.sleep(0.3) finally: control.shutdown() # ~6 at a 50 ms cadence. A spinning loop reaches six figures. assert len(reads) < 30, f"the reporter read the arm {len(reads)} times in 0.3 s" assert reads, "the reporter stopped reading the arm altogether"