diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index e7da307..db3ca14 100644 --- a/parol6/commands/query_commands.py +++ b/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, diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index 5fca560..1bd5def 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.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 diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 763b087..23f8cf5 100644 --- a/parol6/server/controller.py +++ b/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) 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/parol6/server/segment_player.py b/parol6/server/segment_player.py index 6c64a90..93fe9a4 100644 --- a/parol6/server/segment_player.py +++ b/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 diff --git a/parol6/server/state.py b/parol6/server/state.py index da2c408..f127cf1 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 @@ -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 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) diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py index d35d710..561c722 100644 --- a/tests/integration/test_stop_semantics.py +++ b/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 ): diff --git a/tests/unit/test_command_completion_wire.py b/tests/unit/test_command_completion_wire.py index a7bba44..dedd317 100644 --- a/tests/unit/test_command_completion_wire.py +++ b/tests/unit/test_command_completion_wire.py @@ -15,6 +15,7 @@ encode, ) from parol6.server.command_registry import create_command +from parol6.protocol.wire import pack_response from parol6.server.state import ControllerState @@ -25,7 +26,9 @@ def completed(index): command, _, error = create_command(encode(CommandCompletionCmd(index))) assert command is not None, error command.setup(state) - result = decode_message(command.compute(state)).result + # compute() answers with the typed result; the controller is what puts + # it on the wire with the request id it is answering. + result = decode_message(pack_response(command.compute(state), 7)).result assert result.command_index == index assert result.session_id == state.status_session_id return result.completed diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py index b3b8205..965823f 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 ( ActivityCmd, CurrentActionResultStruct, @@ -20,7 +19,7 @@ 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", @@ -40,7 +39,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="", @@ -60,7 +59,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, @@ -82,7 +81,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, @@ -102,7 +101,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,