3 Commits
Author SHA1 Message Date
cristhianandClaude Opus 5 8cd4db93f1 docs: update the test count
validate / markdownlint (pull_request) Successful in 58m43s
validate / validate (pull_request) Successful in 58m48s
validate / test (pull_request) Successful in 58m55s
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
2026-09-15 12:32:43 -05:00
cristhianandClaude Opus 5 4427da9c34 fix requests queued while the arm is down never being answered
_run_control never drained the job queue while the arm had not been reached, so
every request that arrived during an outage waited for a connection that might
never come. That breaks the one-response-per-request rule the node states as
absolute, and it was circular: requests are queued before the arm is reached
because recover is what a caller sends then, yet recover is served by the
control thread and so was the one service that could never run while down.

Serve the queue during the wait between connection attempts. Non-motion services
are tried on the arm, which is what lets recover and clear reach a controller
that answers while refusing to serve. A motion is refused rather than held: the
arm is somewhere unknown and nobody is watching, so a held motion would fire
unattended the moment the link came back.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
2026-09-15 12:32:31 -05:00
cristhianandClaude Opus 5 608e22078e fix zero-velocity jog reporting a running motion as idle
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>
2026-09-15 12:32:17 -05:00
5 changed files with 154 additions and 13 deletions
+7 -2
View File
@@ -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
+1 -1
View File
@@ -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
View File
@@ -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
View File
@@ -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
View File
@@ -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.