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.
242 lines
7.8 KiB
Python
242 lines
7.8 KiB
Python
"""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_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"
|