Compare commits
3
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
8cd4db93f1 | ||
|
|
4427da9c34 | ||
|
|
608e22078e |
@@ -60,7 +60,7 @@ Everything is configured through environment variables — see README.md for the
|
||||
## Test
|
||||
|
||||
```bash
|
||||
uv run pytest # 83 tests, no robot and no dataflow needed
|
||||
uv run pytest # 86 tests, no robot and no dataflow needed
|
||||
uv run ruff check .
|
||||
```
|
||||
|
||||
@@ -85,7 +85,12 @@ while it runs. So:
|
||||
dataflow, and so the arm can be told whether this is a restart.
|
||||
- **`dora_ulite6/control.py`** — `Control`: both arm threads and the mailboxes the loop
|
||||
drains. The **control thread** runs one job at a time and is allowed to block: startup,
|
||||
the `INIT_MODE` move, and every service including the motions that take seconds. The
|
||||
the `INIT_MODE` move, and every service including the motions that take seconds. Its
|
||||
wait between connection attempts serves the queue (`_serve_while_down`) rather than
|
||||
sleeping through it: a request is answered exactly once whatever the arm is doing, and
|
||||
an arm that is down is an outcome to report, not a reason to go quiet. A motion is
|
||||
refused there rather than held, since a held one would fire unattended the moment the
|
||||
link came back. The
|
||||
**reporter thread** never blocks: it reads the report cache at the telemetry cadence,
|
||||
so `tcp_pose` keeps flowing right through a move. **Nothing outside this file may call
|
||||
the driver**, except `stop` and `read_report`, both written for another thread. Same
|
||||
|
||||
@@ -245,7 +245,7 @@ left where it stood.
|
||||
|
||||
```bash
|
||||
uv sync --group dev
|
||||
uv run pytest # 83 tests, no robot needed
|
||||
uv run pytest # 86 tests, no robot needed
|
||||
uv run ruff check .
|
||||
```
|
||||
|
||||
|
||||
+64
-7
@@ -232,10 +232,13 @@ class Control:
|
||||
self._preempt_motions("jog")
|
||||
self._set_state(MOVING)
|
||||
with self._lock:
|
||||
self._jogging = moving
|
||||
was_jogging, self._jogging = self._jogging, moving
|
||||
self._last_jog = time.monotonic()
|
||||
if not moving:
|
||||
self._set_state(READY)
|
||||
# Only a jog that was in fact jogging ends one. A zero velocity from a
|
||||
# teleop source that is merely idling says nothing about a motion that
|
||||
# owns the arm, and must not answer for it.
|
||||
if not moving and was_jogging:
|
||||
self._idle_unless_busy()
|
||||
self._jobs.put(Jog(velocity=velocity))
|
||||
|
||||
def start_motion(self, call: ServiceCall) -> None:
|
||||
@@ -336,11 +339,49 @@ class Control:
|
||||
except RobotUnavailableError as error:
|
||||
log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s")
|
||||
self._set_state(UNAVAILABLE)
|
||||
self._stopping.wait(self.config.reconnect_interval_s)
|
||||
self._serve_while_down(self.config.reconnect_interval_s)
|
||||
return
|
||||
self._started = True
|
||||
self._set_state(READY)
|
||||
|
||||
def _serve_while_down(self, seconds: float) -> None:
|
||||
"""Answer what can be answered without a serving arm, until it is time to retry.
|
||||
|
||||
This is the wait between connection attempts, and it serves the queue
|
||||
rather than sleeping through it. Sitting silent is the one thing a caller
|
||||
must never get: an arm that is down is an outcome to report, not a reason
|
||||
to stop answering, and a request left in the queue here would be answered
|
||||
only if the arm ever came back.
|
||||
"""
|
||||
deadline = time.monotonic() + seconds
|
||||
while not self._stopping.is_set():
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0:
|
||||
return
|
||||
try:
|
||||
job = self._jobs.get(timeout=min(remaining, 0.1))
|
||||
except queue.Empty:
|
||||
continue
|
||||
if isinstance(job, ServiceCall):
|
||||
self._serve_without_arm(job)
|
||||
|
||||
def _serve_without_arm(self, call: ServiceCall) -> None:
|
||||
"""Answer one request that arrived before the arm was serving.
|
||||
|
||||
A motion is refused outright. Holding it would be worse than silence:
|
||||
the arm is somewhere unknown, nobody is watching, and the motion would
|
||||
fire unattended the moment the link came back.
|
||||
|
||||
Everything else is tried on the arm, because a controller that answers
|
||||
while refusing to serve is exactly what `recover` and `clear` are for,
|
||||
and refusing them here would leave the one state they exist to escape
|
||||
with no way out of it.
|
||||
"""
|
||||
if call.moves_the_arm:
|
||||
self._finish_motion(call, FAILED, False, "the arm is not serving yet")
|
||||
return
|
||||
self._respond(call, self._attempt(lambda: services.run(self.robot, call.row)))
|
||||
|
||||
def _start_position(self) -> None:
|
||||
"""Put the arm where INIT_MODE says it should start, if anywhere."""
|
||||
if self.restarted and self.config.init_mode != "none":
|
||||
@@ -403,8 +444,7 @@ class Control:
|
||||
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
|
||||
else:
|
||||
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
|
||||
if self._state == MOVING:
|
||||
self._set_state(READY)
|
||||
self._idle_unless_busy()
|
||||
|
||||
def _leave_jog(self) -> bool:
|
||||
"""Clear the jogging flag, reporting whether the arm was in fact jogging."""
|
||||
@@ -515,6 +555,23 @@ class Control:
|
||||
self._state_changes.append(state)
|
||||
log(f"state: {state}")
|
||||
|
||||
def _idle_unless_busy(self) -> None:
|
||||
"""Report the arm idle, but only when nothing is still driving it.
|
||||
|
||||
One thing ending is not the arm stopping. A motion may still be winding
|
||||
down inside the SDK after the jog that preempted it went quiet; a jog may
|
||||
have taken the arm over from the motion that just finished. Either way
|
||||
`ready` published over it is the false idle the state exists to rule out.
|
||||
|
||||
The state also stops being ours to set once it has left `moving` — that
|
||||
is `unavailable` or `recovering` reporting something this does not know
|
||||
about, and it must not be overwritten with `ready`.
|
||||
"""
|
||||
with self._lock:
|
||||
if self._running_motion is not None or self._jogging or self._state != MOVING:
|
||||
return
|
||||
self._set_state(READY)
|
||||
|
||||
# --- the reporter thread ---
|
||||
|
||||
def _expire_jog(self) -> None:
|
||||
@@ -528,7 +585,7 @@ class Control:
|
||||
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
|
||||
return
|
||||
self._jogging = False
|
||||
self._set_state(READY)
|
||||
self._idle_unless_busy()
|
||||
|
||||
def _run_reporter(self) -> None:
|
||||
"""Publish telemetry, whatever the control thread is doing.
|
||||
|
||||
+6
-1
@@ -30,11 +30,16 @@ class FakeArm:
|
||||
self.recover_error = None
|
||||
self.block = None
|
||||
self.stopped = False
|
||||
self.connected = True
|
||||
|
||||
# --- reads ---
|
||||
|
||||
def is_connected(self):
|
||||
return True
|
||||
return self.connected
|
||||
|
||||
def connect(self):
|
||||
self.calls.append("connect")
|
||||
return self.connected
|
||||
|
||||
def is_ready(self):
|
||||
return self.ready
|
||||
|
||||
+76
-2
@@ -6,17 +6,18 @@ import time
|
||||
import pytest
|
||||
|
||||
from dora_ulite6.config import Config, MotionConfig
|
||||
from dora_ulite6.control import MOVING, PREEMPTED, READY, Control, ServiceCall
|
||||
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, **config):
|
||||
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
|
||||
|
||||
@@ -87,6 +88,56 @@ def test_a_first_start_still_runs_init_mode(monkeypatch):
|
||||
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 ---
|
||||
|
||||
|
||||
@@ -130,6 +181,29 @@ def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch):
|
||||
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.
|
||||
|
||||
Reference in New Issue
Block a user