From 2e103fe9d2fc8b78bd2af4f4d247cd2e16d26f4c Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 02:51:58 -0400 Subject: [PATCH 1/4] Report pending planned commands and clear finished execution indices --- parol6/commands/query_commands.py | 7 ++++++- parol6/server/command_executor.py | 5 +++++ parol6/server/controller.py | 3 ++- parol6/server/segment_player.py | 8 ++++++++ parol6/server/state.py | 3 +++ 5 files changed, 24 insertions(+), 2 deletions(-) diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index c6de72a..bca34be 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -296,7 +296,12 @@ class QueueCommand(QueryCommand[QueueCmd]): def compute(self, state: "ControllerState") -> bytes: return pack_response( QueueResultStruct( - queue=state.queue_nonstreamable, + queue=state.queue_nonstreamable + + [ + name + for index, name in state.pending_planned + if index != state.executing_command_index + ], executing_index=state.executing_command_index, completed_index=state.completed_command_index, last_checkpoint=state.last_checkpoint, diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index 915fc0e..c9c9a2a 100644 --- a/parol6/server/command_executor.py +++ b/parol6/server/command_executor.py @@ -232,6 +232,7 @@ def execute_active_command(self) -> None: except Exception as e: logger.error("Command execution error: %s", e) state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE self._update_queue_state(state) @@ -272,6 +273,7 @@ def _process_tick_result( ) state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE state.completed_command_index = ac.command_index @@ -288,6 +290,7 @@ def _process_tick_result( ) state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE @@ -320,6 +323,7 @@ def cancel_active_command(self, reason: str = "Cancelled by user") -> None: state = self._state_manager.get_state() state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE @@ -335,6 +339,7 @@ def cancel_active_streamable(self) -> bool: if ac and isinstance(ac.command, MotionCommand) and ac.command.streamable: state = self._state_manager.get_state() state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE self.active_command = None diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 621a5de..d88b7b8 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -826,7 +826,7 @@ def _handle_motion_command( # segments are active/queued (e.g. homing), the planner's internal # tracking is correct: Position_in may reflect a mid-motion position # and the planner has already predicted a queued HOME's homed flags. - segment_idle = not self._segment_player.active + segment_idle = not self._segment_player.active and not state.pending_planned pos_snapshot = state.Position_in.copy() if segment_idle else None homed_snapshot: bool | None = None if segment_idle: @@ -843,6 +843,7 @@ def _handle_motion_command( homed=homed_snapshot, ) ) + state.pending_planned.append((cmd_index, cmd_name)) if cmd_type and self._ack_policy.requires_ack(cmd_type): self._reply_ok_index(addr, cmd_index) diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 363861d..9ae1c54 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -293,6 +293,7 @@ def tick(self, state: ControllerState) -> bool: state.collision_pairs = tuple(pairs) if pairs else () state.action_state = ActionState.ERROR state.action_current = "" + state.executing_command_index = -1 state.action_params = "" self._active = None # Halt: cancel all remaining planned work @@ -417,7 +418,10 @@ def _complete_segment(self, seg: Segment, state: ControllerState) -> None: state.queued_duration -= seg.duration state.queued_segments -= 1 state.completed_command_index = final_idx + while state.pending_planned and state.pending_planned[0][0] <= final_idx: + state.pending_planned.popleft() state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE self._active = None @@ -428,6 +432,7 @@ def _on_failure( """Handle inline command failure: set error state, clear buffer, cancel planner.""" state.error = error state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.ERROR self._active = None @@ -472,6 +477,7 @@ def _world_guard( state.collision_pairs = tuple(pairs) if pairs else () state.action_state = ActionState.ERROR state.action_current = "" + state.executing_command_index = -1 state.action_params = "" self._active = None self._buffer.clear() @@ -486,6 +492,7 @@ def cancel(self, state: ControllerState) -> None: # Planned trajectories live here rather than in CommandExecutor. # Cancelling its command cannot clear this player's activity. state.action_current = "" + state.executing_command_index = -1 state.action_params = "" state.action_state = ActionState.IDLE self._active = None @@ -502,5 +509,6 @@ def _drain_planner_queue(self, state: ControllerState) -> None: """Drain any remaining segments from the planner's output queue.""" while self._planner.poll_segment() is not None: pass + state.pending_planned.clear() state.queued_segments = 0 state.queued_duration = 0.0 diff --git a/parol6/server/state.py b/parol6/server/state.py index 3cd0901..81e16d8 100644 --- a/parol6/server/state.py +++ b/parol6/server/state.py @@ -3,6 +3,7 @@ import atexit import logging import secrets +from collections import deque from dataclasses import dataclass, field from typing import Any @@ -257,6 +258,7 @@ class ControllerState: action_state: ActionState = ActionState.IDLE # IDLE, EXECUTING, ERROR action_next: str = "" queue_nonstreamable: list[str] = field(default_factory=list) + pending_planned: deque[tuple[int, str]] = field(default_factory=deque) # Queue progress tracking (monotonically increasing command indices) next_command_index: int = 0 @@ -397,6 +399,7 @@ def reset(self) -> None: self.action_state = ActionState.IDLE self.action_next = "" self.queue_nonstreamable.clear() + self.pending_planned.clear() # Queue progress tracking. next_command_index is deliberately NOT # reset: indices must stay monotonic across reset so a stale From 309eb5f700c82d03c77213c9acef902770a02ec0 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Tue, 8 Sep 2026 03:59:20 -0400 Subject: [PATCH 2/4] test: use controller state for queue query fixtures --- tests/unit/test_query_commands_actions.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py index 56075e8..0bc2d24 100644 --- a/tests/unit/test_query_commands_actions.py +++ b/tests/unit/test_query_commands_actions.py @@ -2,14 +2,13 @@ Unit tests for action-related query commands. Tests ACTIVITY and QUEUE query commands without requiring a running server. -Uses minimal state objects to test command logic in isolation. +Uses the controller state to test command logic in isolation. """ -from types import SimpleNamespace - from waldoctl import ActionState from parol6.commands.query_commands import ActivityCommand, QueueCommand +from parol6.server.state import ControllerState from parol6.protocol.wire import ( CurrentActionResultStruct, ActivityCmd, @@ -29,7 +28,7 @@ def _unpack_response(data: bytes): def test_activity_returns_details(): """Test that ACTIVITY compute() returns correct data.""" - state = SimpleNamespace( + state = ControllerState( action_current="MoveJPoseCommand", action_state=ActionState.EXECUTING, action_next="HomeCommand", @@ -49,7 +48,7 @@ def test_activity_returns_details(): def test_activity_with_idle_state(): """Test ACTIVITY when robot is idle.""" - state = SimpleNamespace( + state = ControllerState( action_current="", action_state=ActionState.IDLE, action_next="", @@ -69,7 +68,7 @@ def test_activity_with_idle_state(): def test_queue_returns_details(): """Test that QUEUE compute() returns correct data.""" - state = SimpleNamespace( + state = ControllerState( queue_nonstreamable=["MoveJPoseCommand", "HomeCommand", "MoveJCommand"], executing_command_index=1, completed_command_index=0, @@ -91,7 +90,7 @@ def test_queue_returns_details(): def test_queue_with_empty_queue(): """Test QUEUE when queue is empty.""" - state = SimpleNamespace( + state = ControllerState( queue_nonstreamable=[], executing_command_index=-1, completed_command_index=-1, @@ -111,7 +110,7 @@ def test_queue_with_empty_queue(): def test_queue_excludes_streamable(): """Test that queue only contains non-streamable commands (by design).""" - state = SimpleNamespace( + state = ControllerState( queue_nonstreamable=["MoveJPoseCommand", "HomeCommand"], executing_command_index=2, completed_command_index=1, From 9125207c1b56de07114dcbb5a8e518c834973bdd Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 05:43:26 +0000 Subject: [PATCH 3/4] Drop segments planned before the last cancel CancelAll travels the planner's command FIFO behind plans already queued, and the worker only clears its blend buffer on it, so commands still in the planner's inbox when Stop arrived were emitted afterwards and played: a stop left an empty queue and an idle executing index for the moment the restart review looked, then the arm moved. Plans now carry the generation they were submitted under; a cancel starts a new one and the proxy drops every segment from an older generation on the way back. Co-Authored-By: Claude Fable 5.1 --- parol6/server/motion_planner.py | 33 ++++++++++++++++++++---- tests/integration/test_stop_semantics.py | 28 ++++++++++++++++++++ 2 files changed, 56 insertions(+), 5 deletions(-) diff --git a/parol6/server/motion_planner.py b/parol6/server/motion_planner.py index 32261d2..689f2b4 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -60,6 +60,7 @@ class TrajectorySegment: command_name: str = "" action_params: str = "" blend_consumed_indices: list[int] = field(default_factory=list) + generation: int = 0 velocity_rad_s: np.ndarray = field(init=False) acceleration_rad_s2: np.ndarray = field(init=False) @@ -82,6 +83,7 @@ class InlineSegment: command_index: int params: object # wire struct (msgspec.Struct — picklable) + generation: int = 0 @dataclass @@ -93,6 +95,7 @@ class ErrorSegment: cartesian_path: np.ndarray | None = None # (N, 6) full TCP path ik_valid: np.ndarray | None = None # (N,) per-pose bool colliding_pairs: list[tuple[str, str]] | None = None # self-collision viz + generation: int = 0 Segment = Union[TrajectorySegment, InlineSegment, ErrorSegment] @@ -112,6 +115,9 @@ class PlanCommand: None # current Position_in (None = use planner internal) ) homed: bool | None = None # all joints homed (None = use planner internal) + # Stamped by the proxy; a cancel starts a new generation and every segment + # planned for an older one is dropped on the way back. + generation: int = 0 @dataclass @@ -574,6 +580,7 @@ class PlannerWorker: def __init__(self, segment_queue: multiprocessing.Queue) -> None: self._segment_queue = segment_queue self._planner = TrajectoryPlanner(diagnostic=False) + self._generation = 0 @property def state(self) -> PlannerState: @@ -586,14 +593,17 @@ def process_command(self, msg: PlanCommand) -> None: if msg.homed is not None: self._planner.state.Homed_in.fill(1 if msg.homed else 0) + self._generation = msg.generation segments = self._planner.process(msg.params, msg.command_index) for seg in segments: + seg.generation = self._generation self._segment_queue.put(seg) def flush_stale_blend(self) -> None: """Flush any pending blend buffer (called on queue timeout).""" segments = self._planner.flush() for seg in segments: + seg.generation = self._generation self._segment_queue.put(seg) def cancel(self) -> None: @@ -761,6 +771,10 @@ def __init__(self) -> None: self._shutdown_event: EventType = multiprocessing.Event() self._ready_event: EventType = multiprocessing.Event() self._process: multiprocessing.Process | None = None + # CancelAll travels the command FIFO behind plans already queued, so + # the worker still emits them after a cancel; the generation is what + # tells those late segments from the next program's. + self._generation = 0 # -- lifecycle -- @@ -832,6 +846,8 @@ def alive(self) -> bool: def submit(self, msg: PlannerMessage) -> None: """Send a message to the planner (non-blocking).""" + if isinstance(msg, PlanCommand): + msg.generation = self._generation self._command_queue.put_nowait(msg) def sync_position(self, position_in: np.ndarray) -> None: @@ -865,16 +881,23 @@ def sync_shapes(self, shapes: list) -> None: def cancel(self) -> None: """Cancel all pending work in the planner.""" + self._generation += 1 self.submit(CancelAll()) # -- planner → main -- def poll_segment(self) -> Segment | None: - """Non-blocking poll for a computed segment. Returns None if empty.""" - try: - return self._segment_queue.get_nowait() - except queue.Empty: - return None + """Non-blocking poll for a computed segment. Returns None if empty. + + Segments planned before the last cancel are discarded here. + """ + while True: + try: + seg = self._segment_queue.get_nowait() + except queue.Empty: + return None + if seg.generation >= self._generation: + return seg def _drain_queue(q: multiprocessing.Queue) -> None: diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py index 2db63dd..8f9a097 100644 --- a/tests/integration/test_stop_semantics.py +++ b/tests/integration/test_stop_semantics.py @@ -89,3 +89,31 @@ def test_estop_latches_until_reset(client: RobotClient, server_proc): "canceled motion resurfaced after reset" ) assert client.home(wait=True, timeout=30.0) >= 0 + + +def test_stop_discards_plans_still_in_the_planner(client: RobotClient, server_proc): + """Commands the planner has not finished planning when Stop arrives must + not play afterwards: a plan finished after the cancel is not a queue.""" + start = client.angles() + assert start is not None + pose = client.pose() + assert pose is not None + away = list(pose) + away[0] += 40.0 + # Cartesian plans take the planner long enough that Stop lands while + # most of these are still in its inbox, behind which CancelAll queues. + for i in range(40): + target = away if i % 2 == 0 else pose + assert client.move_l(target, duration=10.0, wait=False) >= 0 + assert client.stop() == 1 + time.sleep(0.3) + frozen = client.angles() + assert frozen is not None + time.sleep(1.5) + after = client.angles() + assert after is not None + assert np.allclose(after, frozen, atol=0.05), ( + f"a plan finished after Stop played: {frozen} -> {after}" + ) + assert client.queue() == [] + assert client.home(wait=True, timeout=30.0) >= 0 From 2bdc5bb3cde3b2d709c471abc3ab15071258cc57 Mon Sep 17 00:00:00 2001 From: jepson2k <55201008+Jepson2k@users.noreply.github.com> Date: Fri, 11 Sep 2026 23:33:18 +0000 Subject: [PATCH 4/4] Test the queue readback the PR body claimed, against the controller The queue's contents are maintained in three places -- the planner's pending list, the blend consumption that swallows indices, and the executing-index exclusion -- and none of them were exercised: the only test change swapped a namespace for a state object with the pending list left empty. Dropping either mechanism left the suite green. This drives the client against the simulated controller: a paused queue lists what is owed, a resumed one drains to empty, a blend chain takes its consumed indices with it, the executing command is not listed as owed work as well, and a Stop clears the queue and the pause it was holding. Both mechanisms were checked by breaking them. Co-Authored-By: Claude Opus 5 --- tests/integration/test_queue_readback.py | 106 +++++++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 tests/integration/test_queue_readback.py diff --git a/tests/integration/test_queue_readback.py b/tests/integration/test_queue_readback.py new file mode 100644 index 0000000..f23d6a8 --- /dev/null +++ b/tests/integration/test_queue_readback.py @@ -0,0 +1,106 @@ +"""What QUEUE reports, against the simulated controller. + +The readback is what an operator and the frontend's playback bar read to know +what is still owed: commands the planner has accepted but not started, the one +executing now, and nothing at all once a Stop has cleared the queue. Every +assertion here goes through the client and the real controller, because the +pieces it is made of -- the planner's pending list, the blend consumption, the +executing-index exclusion -- are maintained in three different places. +""" + +import numpy as np +import pytest + +from parol6 import RobotClient + + +def _wait(condition, message: str, timeout: float = 10.0) -> None: + import time + + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if condition(): + return + time.sleep(0.02) + pytest.fail(message) + + +def test_the_queue_lists_what_is_owed_and_a_stop_clears_it(client: RobotClient): + start = client.angles() + assert start is not None + first, second = list(start), list(start) + first[0] += 6 + second[0] += 12 + try: + # Paused, so everything accepted stays owed and nothing moves. + assert client.pause() == 1 + held = client.move_j(first, duration=1, wait=False) + queued = client.move_j(second, duration=1, wait=False) + assert held >= 0 and queued > held + _wait( + lambda: len(client.queue() or []) >= 2, + "the paused queue never listed both accepted commands", + ) + listed = client.queue() + assert listed and all(name for name in listed), listed + assert any("MoveJ" in name for name in listed) + assert np.allclose(client.angles(), start, atol=0.05) + + # Resuming drains it: what the queue reports is what is still owed. + assert client.resume() == 1 + assert client.wait_command(queued, timeout=20) + _wait(lambda: client.queue() == [], "the drained queue still reports work") + assert np.allclose(client.angles(), second, atol=0.2) + + # A blended chain is consumed as one motion, and the indices it + # swallowed leave the queue with it rather than lingering as owed work. + assert client.pause() == 1 + corner = list(second) + corner[0] -= 6 + blended = client.move_j(corner, duration=1, r=15, wait=False) + tail = client.move_j(start, duration=1, wait=False) + _wait( + lambda: len(client.queue() or []) >= 2, + "the paused queue never listed the blend chain", + ) + assert client.resume() == 1 + assert client.wait_command(tail, timeout=20) + _wait( + lambda: client.queue() == [], + "the blend's consumed indices stayed in the queue", + ) + # The blended command completed with the chain that swallowed it. + assert blended >= 0 and client.wait_command(blended, timeout=5) + + # With one command executing, the queue reports what is owed after it: + # the executing index is reported in its own field and listing it again + # would double-count the motion the arm is already making. + client.move_j(first, duration=3, wait=False) + trailing = client.move_j(second, duration=1, wait=False) + _wait( + lambda: abs((client.angles() or start)[0] - start[0]) > 0.5, + "the first move never started", + ) + listed = client.queue() + assert listed is not None and len(listed) == 1, ( + f"the executing command is listed as owed work as well: {listed}" + ) + assert client.wait_command(trailing, timeout=25) + + # Stop clears what was owed, and the readback says so immediately. + assert client.pause() == 1 + client.move_j(first, duration=2, wait=False) + client.move_j(second, duration=2, wait=False) + _wait( + lambda: len(client.queue() or []) >= 2, + "the paused queue never listed the commands a Stop must clear", + ) + assert client.stop() == 1 + _wait(lambda: client.queue() == [], "Stop left work in the queue") + assert not client.execution_speed().paused, ( + "Stop drops the pause with the queue it was holding" + ) + finally: + client.stop() + client.resume() + client.set_execution_speed(1)