cristhian
a95d804f0b
`set_cartesian_velocity` passed `duration=velocity_command_ttl_s` on every message, so each one re-armed a timed segment and the controller re-planned the same motion. On a real Lite6 that is not smooth travel: the arm moves in steps. Reducing how often the velocity is sent does not help — re-arming four times a second still steps — so it now uses `duration=0`, the SDK's "always effective, will not stop automatically", and skips a velocity that has not changed. One command stands until a different one replaces it. That removes the controller-side expiry, which `_expire_jog` was relying on: it flipped `_jogging` and reported `ready` without commanding anything, because the controller's own timer had already stopped the arm. With the velocity held indefinitely that would leave an arm travelling while the node reported idle, so `_expire_jog` now queues the stop itself. It goes through the job queue rather than the driver, since it runs on the reporter thread and the control thread owns the driver. `VELOCITY_COMMAND_TTL_S` is unchanged in value and meaning to a caller, but it is now enforced here rather than by the controller. Both halves are one commit on purpose: `duration=0` without the stop in `_expire_jog` is an arm that never stops when its sender dies. Adds tests/test_driver.py — the driver had no direct coverage, because FakeArm replaces it everywhere else, which is why this was invisible to the suite.
331 lines
12 KiB
Python
331 lines
12 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, UNAVAILABLE, Control, ServiceCall
|
|
from dora_ulite6.pose import RobotPose
|
|
from tests.conftest import FakeArm, wait_for
|
|
|
|
|
|
def build(monkeypatch, connected=True, **config):
|
|
"""Return a started Control over a fake arm, and the arms it built."""
|
|
arms = []
|
|
|
|
def make(cfg):
|
|
arm = FakeArm(cfg)
|
|
arm.connected = connected
|
|
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()
|
|
|
|
|
|
# --- an arm that is not serving yet ---
|
|
|
|
|
|
def messages_on(control, output, seen):
|
|
"""Collect messages on one output, draining the outbox as the event loop would."""
|
|
seen.extend(control.take_messages())
|
|
return [message for message in seen if message.output == output]
|
|
|
|
|
|
def test_a_request_sent_while_the_arm_is_down_is_still_answered(monkeypatch):
|
|
# An arm that is down is an outcome to report, not a reason to stop
|
|
# answering. `recover` in particular has to reach a controller that answers
|
|
# while refusing to serve — that state is the one it exists to escape.
|
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
|
seen = []
|
|
try:
|
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
|
|
|
control.submit(ServiceCall(request_id="r1", row={"service": "recover"}))
|
|
|
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
|
assert responses[0].metadata["request_id"] == "r1"
|
|
assert responses[0].fields["ok"] is True
|
|
assert "recover" in control.arms[0].calls
|
|
finally:
|
|
control.shutdown()
|
|
|
|
|
|
def test_a_motion_sent_while_the_arm_is_down_is_refused_rather_than_held(monkeypatch):
|
|
# Holding it would be worse than silence: the arm is somewhere unknown and
|
|
# nobody is watching, so a held motion would fire unattended the moment the
|
|
# link came back.
|
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
|
seen = []
|
|
try:
|
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
|
|
|
control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}))
|
|
|
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
|
assert responses[0].metadata["request_id"] == "m1"
|
|
assert responses[0].fields["ok"] is False
|
|
|
|
control.arms[0].connected = True
|
|
wait_for(lambda: READY in control.take_state_changes())
|
|
assert "go_home" not 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):
|
|
# Nothing tells us a sender died, 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_sender_that_goes_quiet_actually_stops_the_arm(monkeypatch):
|
|
# The velocity is held by the controller indefinitely, so the expiry is the
|
|
# deadman itself and not a note about one. Reporting `ready` without
|
|
# commanding the stop would leave the arm travelling while the node said it
|
|
# was idle, which is worse than not expiring it at all.
|
|
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])
|
|
wait_for(lambda: ("jog", [20.0, 0, 0, 0, 0, 0]) in control.arms[0].calls)
|
|
|
|
wait_for(lambda: ("jog", [0.0] * 6) in control.arms[0].calls)
|
|
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"
|