Author SHA1 Message Date
cristhian a95d804f0b fix jogging stepping by holding one velocity instead of re-arming
validate / validate (pull_request) Successful in 58m41s
validate / test (pull_request) Successful in 58m50s
validate / markdownlint (pull_request) Successful in 58m39s
`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.
2026-09-16 14:27:46 -03:00
cristhian f29265a860 Merge pull request 'Rewrite as a dora 1.0 service node' (#2) from refactor/dora-1.0-services into main
validate / validate (push) Successful in 58m48s
validate / test (push) Successful in 58m53s
validate / markdownlint (push) Successful in 58m43s
Reviewed-on: #2
2026-09-15 16:37:58 +00:00
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
9 changed files with 353 additions and 36 deletions
+16 -2
View File
@@ -60,7 +60,7 @@ Everything is configured through environment variables — see README.md for the
## Test ## Test
```bash ```bash
uv run pytest # 83 tests, no robot and no dataflow needed uv run pytest # 92 tests, no robot and no dataflow needed
uv run ruff check . 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. 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 - **`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, 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, **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 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 the driver**, except `stop` and `read_report`, both written for another thread. Same
@@ -178,6 +183,15 @@ A typed optional input therefore forces the example dataflow to wire it. `reques
## Rules ## Rules
- Do not mention AI tools in commit messages or documentation. - Do not mention AI tools in commit messages or documentation.
- **A velocity is set on the controller once and held.** `set_cartesian_velocity` passes no
duration and skips an unchanged velocity, because re-arming makes the controller re-plan
the same motion and the arm steps instead of travelling — four times a second is enough
to feel it. Do not reintroduce a per-message `duration`, and do not "simplify" the
unchanged-velocity check away.
- **`_expire_jog` is the deadman, not bookkeeping.** Nothing else stops an arm whose sender
died mid-jog: the controller holds the velocity indefinitely. It must command the stop,
not merely report `ready` — an arm travelling while the node says idle is worse than no
expiry at all.
- This controls a real robot. Test changes carefully and preserve the safety checks: the - This controls a real robot. Test changes carefully and preserve the safety checks: the
reachability check before a move, the arrival confirmation after it, and the live error reachability check before a move, the arrival confirmation after it, and the live error
reads that make both of them mean anything. reads that make both of them mean anything.
+15 -7
View File
@@ -123,8 +123,7 @@ you see `done`. Sending them all at once does not queue them; the last one wins.
## Jogging ## Jogging
`jog` is a topic, not a service: a velocity command expires on the controller's own TTL, `jog` is a topic, not a service: a sender keeps sending and nobody waits for an answer.
so a sender keeps sending and nobody waits for an answer.
```python ```python
node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}])) node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
@@ -132,9 +131,18 @@ node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
It still drives the arm, so it behaves like a motion where that matters: the first It still drives the arm, so it behaves like a motion where that matters: the first
non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero
velocity stops it, and so does simply going quiet — the node returns to `ready` once velocity stops it, and so does simply going quiet — the node stops the arm and returns to
`VELOCITY_COMMAND_TTL_S` has passed, because that is when the controller has expired the `ready` once `VELOCITY_COMMAND_TTL_S` has passed.
command.
**The velocity is set once and held.** The controller is given a velocity with no
duration, so it keeps it until a different one arrives: re-sending an unchanged velocity
does not reach the arm at all. Sending it repeatedly re-armed a timed segment and made the
controller re-plan the same motion, which on a real arm is felt as stepping rather than
travel — even at four messages a second.
That also means **the controller no longer stops the arm on its own**, so
`VELOCITY_COMMAND_TTL_S` is enforced here instead. It is a real deadman now rather than a
description of one: a sender that dies mid-jog is what it exists for.
## Telemetry ## Telemetry
@@ -239,13 +247,13 @@ left where it stood.
| `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state | | `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state |
| `HOME_TIMEOUT_S` | `20.0` | How long homing may take | | `HOME_TIMEOUT_S` | `20.0` | How long homing may take |
| `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close | | `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close |
| `VELOCITY_COMMAND_TTL_S` | `0.5` | The controller's own expiry for one `jog` command | | `VELOCITY_COMMAND_TTL_S` | `0.5` | How long a `jog` stream may go quiet before this node stops the arm |
## Development ## Development
```bash ```bash
uv sync --group dev uv sync --group dev
uv run pytest # 83 tests, no robot needed uv run pytest # 92 tests, no robot needed
uv run ruff check . uv run ruff check .
``` ```
+9 -3
View File
@@ -49,9 +49,15 @@ inputs:
description: >- description: >-
A continuous velocity command: a one-row struct {velocity} of 6 numbers A continuous velocity command: a one-row struct {velocity} of 6 numbers
[vx, vy, vz, wx, wy, wz] in mm/s and deg/s. A topic rather than a service [vx, vy, vz, wx, wy, wz] in mm/s and deg/s. A topic rather than a service
because it expires on the controller's own TTL and a sender must keep because a sender keeps sending and nothing is published in answer. It
sending; nothing is published in answer. It drives the arm, so the first drives the arm, so the first non-zero one preempts a motion in play and
non-zero one preempts a motion in play and robot_state turns to moving. robot_state turns to moving.
The velocity is set on the controller once and held: an unchanged one is
not sent to the arm again, because re-arming it makes the controller
re-plan the same motion and the arm step rather than travel. A sender that
goes quiet for longer than VELOCITY_COMMAND_TTL_S is stopped by this node,
which is the deadman behind a sender that dies mid-jog.
outputs: outputs:
response: response:
+4 -3
View File
@@ -60,9 +60,10 @@ class MotionConfig:
ready_timeout_s: float = 3.0 ready_timeout_s: float = 3.0
home_timeout_s: float = 20.0 home_timeout_s: float = 20.0
tool_settle_s: float = 2.0 tool_settle_s: float = 2.0
# The controller's own expiry for one jog command: the arm keeps moving at # How long a jog stream may go quiet before this node stops the arm. The
# the commanded velocity until this runs out, so a sender that stops sending # velocity is set on the controller once and held there indefinitely — see
# stops the arm. # `ULite6.set_cartesian_velocity` — so nothing expires it but us: this is
# the deadman behind a sender that dies mid-jog, enforced by `_expire_jog`.
velocity_command_ttl_s: float = 0.5 velocity_command_ttl_s: float = 0.5
@classmethod @classmethod
+76 -11
View File
@@ -232,10 +232,13 @@ class Control:
self._preempt_motions("jog") self._preempt_motions("jog")
self._set_state(MOVING) self._set_state(MOVING)
with self._lock: with self._lock:
self._jogging = moving was_jogging, self._jogging = self._jogging, moving
self._last_jog = time.monotonic() self._last_jog = time.monotonic()
if not moving: # Only a jog that was in fact jogging ends one. A zero velocity from a
self._set_state(READY) # 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)) self._jobs.put(Jog(velocity=velocity))
def start_motion(self, call: ServiceCall) -> None: def start_motion(self, call: ServiceCall) -> None:
@@ -336,11 +339,49 @@ class Control:
except RobotUnavailableError as error: except RobotUnavailableError as error:
log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s") log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s")
self._set_state(UNAVAILABLE) self._set_state(UNAVAILABLE)
self._stopping.wait(self.config.reconnect_interval_s) self._serve_while_down(self.config.reconnect_interval_s)
return return
self._started = True self._started = True
self._set_state(READY) 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: def _start_position(self) -> None:
"""Put the arm where INIT_MODE says it should start, if anywhere.""" """Put the arm where INIT_MODE says it should start, if anywhere."""
if self.restarted and self.config.init_mode != "none": 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}") self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
else: else:
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields) self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
if self._state == MOVING: self._idle_unless_busy()
self._set_state(READY)
def _leave_jog(self) -> bool: def _leave_jog(self) -> bool:
"""Clear the jogging flag, reporting whether the arm was in fact jogging.""" """Clear the jogging flag, reporting whether the arm was in fact jogging."""
@@ -515,20 +555,45 @@ class Control:
self._state_changes.append(state) self._state_changes.append(state)
log(f"state: {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 --- # --- the reporter thread ---
def _expire_jog(self) -> None: def _expire_jog(self) -> None:
"""Stop calling the arm jogging once the controller has expired the command. """Stop the arm once the sender driving it has gone quiet.
A jog lives on the controller's own TTL, so a sender that goes quiet **This is the deadman, not bookkeeping.** The velocity is set on the
leaves an arm that stopped moving a moment later. Nothing tells us; the controller once and held there indefinitely — see
clock does. :meth:`ULite6.set_cartesian_velocity` for why it cannot be given a
duration — so a sender that dies mid-jog leaves an arm that travels
until something says otherwise. Nothing tells us; the clock does, and
this is what says otherwise.
The stop goes through the job queue rather than the arm directly: this
runs on the reporter thread, and the control thread is the only one that
may touch the driver.
""" """
with self._lock: with self._lock:
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s: if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
return return
self._jogging = False self._jogging = False
self._set_state(READY) self._jobs.put(Jog(velocity=[0.0] * 6))
self._idle_unless_busy()
def _run_reporter(self) -> None: def _run_reporter(self) -> None:
"""Publish telemetry, whatever the control thread is doing. """Publish telemetry, whatever the control thread is doing.
+30 -5
View File
@@ -61,6 +61,10 @@ class ULite6:
# falls back to a blocking get_position(). # falls back to a blocking get_position().
self.arm = XArmAPI(self.ip, do_not_open=True, enable_report=True) self.arm = XArmAPI(self.ip, do_not_open=True, enable_report=True)
self._velocity_mode = False self._velocity_mode = False
# The velocity currently standing on the controller. A velocity is a
# set-point it holds, so re-sending an unchanged one is not a no-op: see
# `set_cartesian_velocity`.
self._commanded: list | None = None
self.connect() self.connect()
# --- connection --- # --- connection ---
@@ -500,10 +504,24 @@ class ULite6:
def set_cartesian_velocity(self, velocity: list) -> None: def set_cartesian_velocity(self, velocity: list) -> None:
"""Drive the arm at a constant base-frame velocity (xArm mode 5) and return. """Drive the arm at a constant base-frame velocity (xArm mode 5) and return.
``velocity`` is ``[vx, vy, vz, wx, wy, wz]`` in mm/s and deg/s. The arm ``velocity`` is ``[vx, vy, vz, wx, wy, wz]`` in mm/s and deg/s. The
keeps moving until the controller's own TTL expires or a new velocity controller holds it until a different one arrives or an all-zero vector
arrives, so a sender must keep sending; an all-zero vector stops it and stops it and leaves velocity mode.
leaves velocity mode.
**The velocity is set once and then left alone.** ``duration=0`` is the
SDK's "always effective, will not stop automatically", and an unchanged
velocity is not sent again at all. Both halves matter: this used to pass
``duration=velocity_command_ttl_s`` on every message, which re-armed a
timed segment several times a second and made the controller re-plan the
same motion over and over. On a real arm that is not smooth travel — an
operator feels it as the arm moving in steps. Re-arming even four times a
second is enough to produce it, so the answer is not to re-arm less
often but not to re-arm at all.
Dropping ``duration`` means the controller no longer stops the arm by
itself, so the expiry that backs a sender going quiet has to be enforced
here instead: :meth:`Control._expire_jog` commands the stop that the
controller's timer used to.
""" """
self._require_connection() self._require_connection()
if any(velocity): if any(velocity):
@@ -511,12 +529,19 @@ class ULite6:
self.arm.set_mode(MODE_CARTESIAN_VELOCITY) self.arm.set_mode(MODE_CARTESIAN_VELOCITY)
self.arm.set_state(0) self.arm.set_state(0)
self._velocity_mode = True self._velocity_mode = True
# Every route out of velocity mode comes back through here, so
# this is the one place the standing velocity has to be
# forgotten — re-entering the mode must always re-send.
self._commanded = None
if velocity == self._commanded:
return
code = self.arm.vc_set_cartesian_velocity( code = self.arm.vc_set_cartesian_velocity(
velocity, velocity,
is_radian=False, is_radian=False,
is_tool_coord=False, is_tool_coord=False,
duration=self.motion.velocity_command_ttl_s, duration=0,
) )
self._commanded = list(velocity)
self._check_motion_code(code, "set_cartesian_velocity") self._check_motion_code(code, "set_cartesian_velocity")
return return
if self._velocity_mode: if self._velocity_mode:
+6 -1
View File
@@ -30,11 +30,16 @@ class FakeArm:
self.recover_error = None self.recover_error = None
self.block = None self.block = None
self.stopped = False self.stopped = False
self.connected = True
# --- reads --- # --- reads ---
def is_connected(self): def is_connected(self):
return True return self.connected
def connect(self):
self.calls.append("connect")
return self.connected
def is_ready(self): def is_ready(self):
return self.ready return self.ready
+104
View File
@@ -0,0 +1,104 @@
"""The driver's half of velocity control: what actually reaches the controller.
`FakeArm` stands in for this whole class everywhere else, so nothing else in the
suite can see what the driver says to the SDK. That is the one thing worth
pinning here: a velocity is a set-point the controller holds, and how often it is
re-sent is the difference between an arm that travels and an arm that steps.
"""
import pytest
from dora_ulite6.config import Config
from dora_ulite6.driver import ULite6
MOVE = [20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
BACK = [-20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
STILL = [0.0] * 6
class FakeSDK:
"""Just enough XArmAPI to build a driver and watch it command a velocity."""
def __init__(self, *args, **kwargs):
self.connected = True
self.mode = None
self.velocities = []
def connect(self):
self.connected = True
def disconnect(self):
self.connected = False
def motion_enable(self, enable=True):
pass
def set_mode(self, mode):
self.mode = mode
def set_state(self, state):
pass
def vc_set_cartesian_velocity(self, speeds, is_radian=None, is_tool_coord=False, duration=-1):
self.velocities.append((list(speeds), duration))
return 0
@pytest.fixture
def robot(monkeypatch):
"""Return a driver over a fake SDK, with the calls it made on `arm`."""
monkeypatch.setattr("dora_ulite6.driver.XArmAPI", FakeSDK)
return ULite6(Config())
def commanded(robot):
"""Return every velocity the driver put on the controller."""
return [speeds for speeds, _ in robot.arm.velocities]
def test_a_velocity_is_never_given_a_duration(robot):
# `duration` makes the controller stop the arm on its own, which means every
# message re-arms a timed segment and the controller re-plans the same
# motion. The SDK reads 0 as "always effective, will not stop
# automatically", which is what lets one command stand.
robot.set_cartesian_velocity(MOVE)
assert [duration for _, duration in robot.arm.velocities] == [0]
def test_a_held_velocity_reaches_the_controller_once(robot):
# The arm holds it, so saying it again says nothing new — and saying it
# again several times a second is what an operator feels as stepping.
for _ in range(5):
robot.set_cartesian_velocity(MOVE)
assert commanded(robot) == [MOVE]
def test_a_changed_velocity_reaches_the_controller(robot):
# Sending once is worth nothing if a new direction is taken for the old one.
robot.set_cartesian_velocity(MOVE)
robot.set_cartesian_velocity(BACK)
assert commanded(robot) == [MOVE, BACK]
def test_a_zero_velocity_stops_the_arm_and_leaves_velocity_mode(robot):
robot.set_cartesian_velocity(MOVE)
robot.set_cartesian_velocity(STILL)
assert commanded(robot) == [MOVE, STILL]
assert robot._velocity_mode is False
def test_the_same_velocity_is_sent_again_after_leaving_velocity_mode(robot):
# Leaving the mode clears what the controller was holding, so the velocity
# it had before means nothing on the way back in. Remembering it across the
# gap would leave the arm still while the operator held a direction.
robot.set_cartesian_velocity(MOVE)
robot.set_cartesian_velocity(STILL)
robot.set_cartesian_velocity(MOVE)
assert commanded(robot) == [MOVE, STILL, MOVE]
+93 -4
View File
@@ -6,17 +6,18 @@ import time
import pytest import pytest
from dora_ulite6.config import Config, MotionConfig 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 dora_ulite6.pose import RobotPose
from tests.conftest import FakeArm, wait_for 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.""" """Return a started Control over a fake arm, and the arms it built."""
arms = [] arms = []
def make(cfg): def make(cfg):
arm = FakeArm(cfg) arm = FakeArm(cfg)
arm.connected = connected
arms.append(arm) arms.append(arm)
return arm return arm
@@ -87,6 +88,56 @@ def test_a_first_start_still_runs_init_mode(monkeypatch):
control.shutdown() 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 --- # --- jogging ---
@@ -130,9 +181,31 @@ def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch):
control.shutdown() 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): 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 # Nothing tells us a sender died, so the clock has to.
# clock has to.
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02)) control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
try: try:
wait_for(lambda: READY in control.take_state_changes()) wait_for(lambda: READY in control.take_state_changes())
@@ -144,6 +217,22 @@ def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
control.shutdown() 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): def test_a_jog_preempts_a_motion_that_is_running(monkeypatch):
control = build(monkeypatch) control = build(monkeypatch)
control.arms[0].block = threading.Event() control.arms[0].block = threading.Event()