Report accepted queued commands and clear finished execution indices

parol6/commands/query_commands.py

@@ -300,7 +300,12 @@ class QueueCommand(QueryCommand[QueueCmd]):
 
     def compute(self, state: "ControllerState") -> Response:
         return 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,

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.record_completion(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

parol6/server/controller.py

@@ -867,7 +867,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:
@@ -884,6 +884,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(req_id, addr, cmd_index)
 

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:

parol6/server/segment_player.py

@@ -300,6 +300,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
@@ -416,14 +417,20 @@ def _tick_inline(self, seg: InlineSegment, state: ControllerState) -> bool | Non
 
     def _complete_segment(self, seg: Segment, state: ControllerState) -> None:
         """Mark segment as completed and update tracking indices."""
+        final_idx = seg.command_index
         if isinstance(seg, TrajectorySegment):
             for idx in seg.blend_consumed_indices:
                 if idx != seg.command_index:
                     state.record_completion(idx)
+                if idx > final_idx:
+                    final_idx = idx
             state.queued_duration -= seg.duration
         state.queued_segments -= 1
         state.record_completion(seg.command_index)
+        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
@@ -434,6 +441,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
@@ -478,6 +486,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()
@@ -492,6 +501,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
@@ -508,5 +518,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

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
@@ -412,6 +414,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

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)