cristhianandClaude Opus 5
608e22078e
start_jog restored READY for any zero velocity, without checking whether a motion owned the arm. A teleop source streaming zero-velocity ticks during a move published moving -> ready mid-motion, and the closing guard in _run_motion then saw READY rather than MOVING and never republished, so a caller watching robot_state concluded the motion had finished early. Route every "the arm has stopped" transition through _idle_unless_busy, which reports READY only when no motion is running, no jog is driving the arm, and the state has not already left MOVING for unavailable or recovering. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
265 lines
8.7 KiB
Python
265 lines
8.7 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_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"
|