Report accepted queued commands and clear finished execution indices

parol6/commands/query_commands.py

@@ -315,7 +315,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,

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

@@ -828,7 +828,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:
@@ -845,6 +845,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)
 

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

@@ -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
@@ -409,14 +410,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
@@ -427,6 +434,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
@@ -471,6 +479,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()
@@ -485,6 +494,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
@@ -501,5 +511,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_stop_semantics.py

@@ -91,6 +91,34 @@ def test_estop_latches_until_reset(client: RobotClient, server_proc):
     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
+
+
 def test_stop_discards_a_queued_tcp_transform_from_the_planner_too(
     client: RobotClient, server_proc
 ):

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,