diff --git a/dora_ulite6/control.py b/dora_ulite6/control.py index ba45db7..8cc93c5 100644 --- a/dora_ulite6/control.py +++ b/dora_ulite6/control.py @@ -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. diff --git a/tests/test_lifecycle.py b/tests/test_lifecycle.py index 044b585..ba66ab7 100644 --- a/tests/test_lifecycle.py +++ b/tests/test_lifecycle.py @@ -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.