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>
This commit is contained in:
+25
-6
@@ -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:
|
||||
@@ -403,8 +406,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 +517,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 +547,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.
|
||||
|
||||
@@ -130,6 +130,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