diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 2deb928..3ad438b 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -1586,11 +1586,11 @@ async def wait_status( if remaining <= 0: return False try: - await asyncio.wait_for( - self._status_event.wait(), - timeout=min(remaining, 0.5), - ) - except asyncio.TimeoutError: + # asyncio.timeout, not wait_for: on 3.11 wait_for can swallow an + # outer cancellation when its child wakes in the same turn. + async with asyncio.timeout(min(remaining, 0.5)): + await self._status_event.wait() + except (asyncio.TimeoutError, TimeoutError): continue if self._closed: @@ -1689,8 +1689,12 @@ async def _await_completion_hint(self, command_index: int, timeout: float) -> No if remaining <= 0: return try: - await asyncio.wait_for(self._status_event.wait(), remaining) - except asyncio.TimeoutError: + # asyncio.timeout, not wait_for: on 3.11 wait_for can swallow an + # outer cancellation when its child wakes in the same turn, and + # the caller's deadline rides on that cancellation. + async with asyncio.timeout(remaining): + await self._status_event.wait() + except (asyncio.TimeoutError, TimeoutError): return # --------------- Move commands (queued, pre-computed trajectory) --------------- diff --git a/parol6/commands/query_commands.py b/parol6/commands/query_commands.py index 8e77828..bc56679 100644 --- a/parol6/commands/query_commands.py +++ b/parol6/commands/query_commands.py @@ -301,7 +301,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/motion/trajectory.py b/parol6/motion/trajectory.py index 74288c6..24c7c9a 100644 --- a/parol6/motion/trajectory.py +++ b/parol6/motion/trajectory.py @@ -50,6 +50,54 @@ def _rad_to_steps_alloc(rad: NDArray) -> NDArray[np.int32]: return out +def _trapezoid_duration(distance: float, v_max: float, a_max: float) -> float: + """Duration of a trapezoidal profile over ``distance``, starting and ending at rest.""" + distance = abs(distance) + if distance < 1e-12: + return 0.0 + if distance * a_max >= v_max * v_max: + return v_max / a_max + distance / v_max + return 2.0 * float(np.sqrt(distance / a_max)) + + +def _trapezoid_samples( + times: NDArray[np.float64], q0: float, q1: float, v_max: float, a_max: float +) -> NDArray[np.float64]: + """Sample a trapezoidal profile from ``q0`` to ``q1``, starting and ending at rest.""" + distance = abs(q1 - q0) + duration = _trapezoid_duration(distance, v_max, a_max) + if duration <= 0.0: + return np.full(times.shape, q0, dtype=np.float64) + + if distance * a_max >= v_max * v_max: + t_accel = v_max / a_max + v_peak = v_max + else: + t_accel = duration / 2.0 + v_peak = a_max * t_accel + + t = np.clip(times, 0.0, duration) + t_decel = duration - t_accel + travelled = np.where( + t < t_accel, + 0.5 * a_max * t * t, + np.where( + t < t_decel, + 0.5 * v_peak * t_accel + v_peak * (t - t_accel), + distance - 0.5 * a_max * (duration - t) ** 2, + ), + ) + return q0 + np.sign(q1 - q0) * travelled + + +def _quintic_samples( + times: NDArray[np.float64], q0: float, q1: float, duration: float +) -> NDArray[np.float64]: + """Sample a quintic profile from ``q0`` to ``q1``, at rest and unaccelerated at both ends.""" + s = np.clip(times / duration, 0.0, 1.0) + return q0 + (q1 - q0) * s * s * s * (10.0 - 15.0 * s + 6.0 * s * s) + + class _LinearPath: """Piecewise linear path wrapper for TOPPRA compatibility. @@ -742,15 +790,10 @@ def _compute_joint_duration_trapezoid(self) -> float: """ Compute duration for joint paths using trapezoidal profile. - For each joint, uses InterpolatePy to compute the minimum duration - for its displacement given its velocity/acceleration limits. + For each joint, computes the minimum duration for its displacement + given its velocity/acceleration limits. Returns the maximum (slowest joint determines overall duration). """ - from interpolatepy.trapezoidal import ( - TrajectoryParams as TrapParams, - TrapezoidalTrajectory, - ) - positions = self.joint_path.positions if len(positions) < 2: return self.dt * 2 @@ -763,15 +806,7 @@ def _compute_joint_duration_trapezoid(self) -> float: if delta < 1e-6: continue - params = TrapParams( - q0=0.0, - q1=delta, - v0=0.0, - v1=0.0, - vmax=self.v_max[j], - amax=self.a_max[j], - ) - _, duration = TrapezoidalTrajectory.generate_trajectory(params) + duration = _trapezoid_duration(delta, self.v_max[j], self.a_max[j]) max_duration = max(max_duration, duration) return max(max_duration, self.dt * 2) @@ -879,8 +914,6 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: Each joint independently follows a quintic polynomial profile, synchronized to finish at the same time. """ - from interpolatepy import BoundaryCondition, PolynomialTrajectory, TimeInterval - start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] @@ -899,17 +932,9 @@ def _build_quintic_trajectory_joint(self) -> Trajectory: trajectory_rad[:, j] = start_pos[j] continue - bc_start = BoundaryCondition( - position=start_pos[j], velocity=0.0, acceleration=0.0 - ) - bc_end = BoundaryCondition( - position=end_pos[j], velocity=0.0, acceleration=0.0 + trajectory_rad[:, j] = _quintic_samples( + times, start_pos[j], end_pos[j], duration ) - interval = TimeInterval(start=0.0, end=duration) - traj = PolynomialTrajectory.order_5_trajectory(bc_start, bc_end, interval) - - for i, t in enumerate(times): - trajectory_rad[i, j] = traj(t)[0] trajectory_rad, duration = self._enforce_segment_limits( trajectory_rad, duration @@ -926,8 +951,6 @@ def _build_quintic_trajectory_cartesian(self) -> Trajectory: TCP follows quintic polynomial profile along the path, with local slowdown where velocity limits would be exceeded. """ - from interpolatepy import BoundaryCondition, PolynomialTrajectory, TimeInterval - if self.duration: duration = self.duration else: @@ -935,17 +958,10 @@ def _build_quintic_trajectory_cartesian(self) -> Trajectory: duration = self._compute_cartesian_duration_from_path() # Quintic profile for the path parameter s, from s=0 to s=1 - bc_start = BoundaryCondition(position=0.0, velocity=0.0, acceleration=0.0) - bc_end = BoundaryCondition(position=1.0, velocity=0.0, acceleration=0.0) - interval = TimeInterval(start=0.0, end=duration) - traj = PolynomialTrajectory.order_5_trajectory(bc_start, bc_end, interval) - n_output = max(2, int(np.ceil(duration / self.dt))) times = np.linspace(0.0, duration, n_output) - profile_s = np.empty(n_output, dtype=np.float64) - for i in range(n_output): - profile_s[i] = traj(float(times[i]))[0] + profile_s = _quintic_samples(times, 0.0, 1.0, duration) trajectory_rad = self.joint_path.sample_many(profile_s) @@ -976,11 +992,6 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: Each joint independently follows a trapezoidal velocity profile, synchronized to finish at the same time. """ - from interpolatepy.trapezoidal import ( - TrajectoryParams as TrapParams, - TrapezoidalTrajectory, - ) - start_pos = self.joint_path.positions[0] end_pos = self.joint_path.positions[-1] @@ -999,23 +1010,18 @@ def _build_trapezoid_trajectory_joint(self) -> Trajectory: trajectory_rad[:, j] = start_pos[j] continue - params = TrapParams( - q0=start_pos[j], - q1=end_pos[j], - v0=0.0, - v1=0.0, - vmax=self.v_max[j], - amax=self.a_max[j], - ) - traj_fn, profile_duration = TrapezoidalTrajectory.generate_trajectory( - params - ) + profile_duration = _trapezoid_duration(delta, self.v_max[j], self.a_max[j]) # Scale this joint's own profile time onto the synchronized duration time_scale = profile_duration / duration if duration > 0 else 1.0 - for i, t in enumerate(times): - trajectory_rad[i, j] = traj_fn(t * time_scale)[0] + trajectory_rad[:, j] = _trapezoid_samples( + times * time_scale, + start_pos[j], + end_pos[j], + self.v_max[j], + self.a_max[j], + ) trajectory_rad, duration = self._enforce_segment_limits( trajectory_rad, duration @@ -1032,11 +1038,6 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: TCP follows trapezoidal velocity profile along the path, with local slowdown where velocity limits would be exceeded. """ - from interpolatepy.trapezoidal import ( - TrajectoryParams as TrapParams, - TrapezoidalTrajectory, - ) - if self.duration: duration = self.duration else: @@ -1046,15 +1047,7 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: vmax_s, amax_s, _ = self._compute_s_profile_limits() # Trapezoidal profile for the path parameter s, from s=0 to s=1 - params = TrapParams( - q0=0.0, - q1=1.0, - v0=0.0, - v1=0.0, - vmax=vmax_s, - amax=amax_s, - ) - traj_fn, profile_duration = TrapezoidalTrajectory.generate_trajectory(params) + profile_duration = _trapezoid_duration(1.0, vmax_s, amax_s) # If user specified longer duration, scale to match if self.duration and self.duration > profile_duration: @@ -1067,9 +1060,7 @@ def _build_trapezoid_trajectory_cartesian(self) -> Trajectory: n_output = max(2, int(np.ceil(duration / self.dt))) times = np.linspace(0.0, duration, n_output) - profile_s = np.array( - [traj_fn(t * time_scale)[0] for t in times], dtype=np.float64 - ) + profile_s = _trapezoid_samples(times * time_scale, 0.0, 1.0, vmax_s, amax_s) trajectory_rad = self.joint_path.sample_many(profile_s) diff --git a/parol6/server/command_executor.py b/parol6/server/command_executor.py index 5fca560..b36186b 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) @@ -320,6 +322,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 +338,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 a252672..2b00c5a 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -859,7 +859,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: @@ -876,6 +876,7 @@ def _handle_motion_command( homed=homed_snapshot, ) ) + state.pending_planned.append((cmd_index, cmd_name)) state.plan_submitted_index = cmd_index 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 e646d4a..9e641ed 100644 --- a/parol6/server/motion_planner.py +++ b/parol6/server/motion_planner.py @@ -735,6 +735,7 @@ def motion_planner_main( ErrorSegment( command_index=msg.command_index, error=robot_error, + generation=msg.generation, ) ) worker.cancel() diff --git a/parol6/server/segment_player.py b/parol6/server/segment_player.py index 65106d8..0ea94f5 100644 --- a/parol6/server/segment_player.py +++ b/parol6/server/segment_player.py @@ -370,6 +370,15 @@ def _activate_next(self, state: ControllerState) -> None: self._inline_activated = False state.executing_command_index = self._active.command_index state.action_state = ActionState.EXECUTING + # The chain this segment plays is no longer owed: QUEUE lists only + # commands not yet started. + started = seg.command_index + if isinstance(seg, TrajectorySegment): + for idx in seg.blend_consumed_indices: + if idx > started: + started = idx + while state.pending_planned and state.pending_planned[0][0] <= started: + state.pending_planned.popleft() # Populate action info for trajectory segments (inline segments set these later) if isinstance(self._active, TrajectorySegment): self._position_rad[:] = self._active.trajectory_rad[0] @@ -422,14 +431,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 @@ -498,6 +513,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 @@ -514,6 +530,7 @@ 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 state.plan_received_index = state.plan_submitted_index diff --git a/parol6/server/state.py b/parol6/server/state.py index e84fc61..41bc1f8 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 @@ -416,6 +418,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/pyproject.toml b/pyproject.toml index dcc440e..92fe83a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -40,7 +40,6 @@ dependencies = [ "scipy>=1.11.4", "ruckig>=0.12.2", "toppra>=0.6.3", - "interpolatepy>=2.0.0", "numpy>=2.0,<2.5", # numba (0.6x) requires numpy<2.5 "numba>=0.59", "psutil>=5.9", diff --git a/tests/conftest.py b/tests/conftest.py index 83bea68..feaee5c 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -6,8 +6,10 @@ """ import logging +import math import os import socket +import time from collections.abc import Generator from dataclasses import dataclass @@ -27,6 +29,23 @@ def free_udp_port() -> int: return sock.getsockname()[1] +def wait_until(pred, timeout: float, msg: str) -> None: + """Poll ``pred`` until it holds, failing the test with ``msg`` at ``timeout``.""" + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if pred(): + return + time.sleep(0.02) + pytest.fail(msg) + + +def rows_for(seconds: float) -> int: + """Rows a hold of ``seconds`` occupies in a dry-run record from its first tick.""" + from parol6.client.dry_run_client import _STRIDE + + return math.ceil(round(seconds / cfg.INTERVAL_S) / _STRIDE) + + logger = logging.getLogger(__name__) diff --git a/tests/integration/test_queue_readback.py b/tests/integration/test_queue_readback.py new file mode 100644 index 0000000..d44b776 --- /dev/null +++ b/tests/integration/test_queue_readback.py @@ -0,0 +1,110 @@ +"""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 + # The chain plays as one motion: while its head executes, the command + # it swallowed is not owed work either. + assert client.wait_status(lambda s: s.executing_index == blended, timeout=5) + assert client.queue() == [], "a consumed blend member was listed as owed" + 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_shapes_e2e.py b/tests/integration/test_shapes_e2e.py index c67dfea..10997c5 100644 --- a/tests/integration/test_shapes_e2e.py +++ b/tests/integration/test_shapes_e2e.py @@ -18,7 +18,7 @@ from parol6 import MotionError, RobotClient -from tests.conftest import free_udp_port +from tests.conftest import free_udp_port, wait_until from waldoctl import Box, Physical pytestmark = pytest.mark.integration @@ -106,7 +106,7 @@ def test_attached_part_blocks_motion_except_for_declared_contacts(client: RobotC assert client.shapes().program[-1] == part assert client.estop() == 1 - _wait_until( + wait_until( lambda: not client.shapes().attachments_valid, 3.0, "attachment remained valid after stop", @@ -183,15 +183,6 @@ def _wrist_box(target_deg: list[float], name: str) -> Box: ) -def _wait_until(pred, timeout: float, msg: str) -> None: - deadline = time.monotonic() + timeout - while time.monotonic() < deadline: - if pred(): - return - time.sleep(0.02) - pytest.fail(msg) - - def _j1(client: RobotClient) -> float: angles = client.angles() assert angles is not None @@ -252,18 +243,23 @@ def test_set_shapes_mid_flight_halts_streaming_move(client: RobotClient, server_ try: idx = client.move_j(target, duration=4.0, wait=False) assert idx >= 0 - _wait_until( + wait_until( lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming" ) assert client.set_shapes([_wrist_box(target, "blocker")]) == 1 - _wait_until( + wait_until( lambda: client.error() is not None, 3.0, "world change never halted the move", ) err = client.error() assert err is not None and "shape:blocker" in err.cause + # The ERROR frame names the halted command, so a log keyed on the + # executing index sees the failure. + assert client.wait_status( + lambda s: s.executing_index == idx and s.error is not None, timeout=2.0 + ), "the ERROR frame dropped the halted command's index" j1_stop = _j1(client) assert j1_stop > 30.0, f"arm reached the keep-out region (J1={j1_stop:.1f})" @@ -286,13 +282,13 @@ def test_set_shapes_mid_flight_rejects_queued_move_at_activation( i1 = client.move_j(t1, duration=1.5, wait=False) i2 = client.move_j(t2, duration=2.0, wait=False) assert i1 >= 0 and i2 >= 0 - _wait_until( + wait_until( lambda: _j1(client) < HOME_J1 - 2.0, 10.0, "first move never started" ) assert client.set_shapes([_wrist_box(t2, "late-wall")]) == 1 - _wait_until( + wait_until( lambda: client.error() is not None, 10.0, "queued move was never invalidated", @@ -300,7 +296,7 @@ def test_set_shapes_mid_flight_rejects_queued_move_at_activation( err = client.error() assert err is not None and "shape:late-wall" in err.cause # The clear first move finished; the blocked second never streamed. - _wait_until( + wait_until( lambda: abs(_j1(client) - 60.0) < 2.0, 10.0, f"arm not at the first target (J1={_j1(client):.1f})", @@ -321,7 +317,7 @@ def test_set_shapes_mid_flight_off_path_does_not_disturb_motion( try: idx = client.move_j(target, duration=2.5, wait=False) assert idx >= 0 - _wait_until( + wait_until( lambda: _j1(client) < HOME_J1 - 5.0, 10.0, "move never started streaming" ) diff --git a/tests/integration/test_stale_error_wait.py b/tests/integration/test_stale_error_wait.py index fa34301..335b584 100644 --- a/tests/integration/test_stale_error_wait.py +++ b/tests/integration/test_stale_error_wait.py @@ -8,26 +8,16 @@ "failing" with a stale self-collision error as the arm visibly homed). """ -import time - import numpy as np import pytest from parol6 import MotionError, RobotClient +from tests.conftest import wait_until from waldoctl import Box pytestmark = pytest.mark.integration -def _wait_until(pred, timeout: float, msg: str) -> None: - deadline = time.monotonic() + timeout - while time.monotonic() < deadline: - if pred(): - return - time.sleep(0.02) - pytest.fail(msg) - - def _reject_move(client: RobotClient) -> None: """Drive a real guard rejection: a keep-out enveloping the target wrist.""" import parol6.PAROL6_ROBOT as PAROL6_ROBOT @@ -63,7 +53,7 @@ def test_streaming_accept_clears_stale_error(client: RobotClient, server_proc): _reject_move(client) assert client.error() is not None assert client.jog_j(0, 0.2, 0.2) >= 0 - _wait_until( + wait_until( lambda: client.error() is None, 2.0, "jog accept never cleared the stale error", diff --git a/tests/integration/test_stop_semantics.py b/tests/integration/test_stop_semantics.py index 561c722..b73bece 100644 --- a/tests/integration/test_stop_semantics.py +++ b/tests/integration/test_stop_semantics.py @@ -8,12 +8,14 @@ subsequent command. """ +import socket import time import numpy as np import pytest from parol6 import MotionError, RobotClient +from parol6.protocol.wire import SelectToolCmd, encode_command pytestmark = pytest.mark.integration @@ -119,6 +121,31 @@ def test_stop_discards_plans_still_in_the_planner(client: RobotClient, server_pr assert client.home(wait=True, timeout=30.0) >= 0 +def test_a_planner_failure_after_a_stop_still_surfaces( + client: RobotClient, server_proc, ports +): + """A Stop starts a new planner generation; a command the planner itself + fails on afterwards must still report, not vanish as pre-stop work.""" + assert client.stop() == 1 + assert client.error() is None + # The client uppercases tool names; the worker applies the raw name, so a + # lowercase one passes wire validation and fails inside the planner. + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as raw: + raw.sendto( + encode_command(SelectToolCmd(tool_name="none")), + (ports.server_ip, ports.server_port), + ) + deadline = time.monotonic() + 5 + while client.error() is None: + assert time.monotonic() < deadline, ( + "the planner failure never surfaced after a Stop" + ) + time.sleep(0.02) + error = client.error() + assert error is not None and "Unknown tool" in error.cause + assert client.queue() == [] + + 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 c265ded..f06b969 100644 --- a/tests/unit/test_command_completion_wire.py +++ b/tests/unit/test_command_completion_wire.py @@ -26,6 +26,8 @@ def completed(index): command, _, error = create_command(encode(CommandCompletionCmd(index))) assert command is not None, error command.setup(state) + # 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 diff --git a/tests/unit/test_dry_run_record.py b/tests/unit/test_dry_run_record.py index 44ace1e..a08a80d 100644 --- a/tests/unit/test_dry_run_record.py +++ b/tests/unit/test_dry_run_record.py @@ -1,14 +1,12 @@ """The commanded record a parol6 dry run returns: one block per command on one row axis, with delays, tool travel and refusals all on it.""" -import math - import numpy as np import pytest from waldoctl import following_error -from parol6.client.dry_run_client import _STRIDE, DryRunRobotClient -from parol6.config import INTERVAL_S +from parol6.client.dry_run_client import DryRunRobotClient +from tests.conftest import rows_for from parol6.tools import get_registry HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] @@ -16,11 +14,6 @@ W2 = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0] -def _rows_for(seconds: float) -> int: - """Rows a hold of *seconds* occupies from the program's first tick.""" - return math.ceil(round(seconds / INTERVAL_S) / _STRIDE) - - def _span(record, block): return slice(block.start_row, block.start_row + block.rows) @@ -30,7 +23,7 @@ def test_delay_holds_the_pose_for_its_rows(): index = client.delay(2.0) record = client.plan() block = record.blocks[index] - assert block.command == index and block.rows == _rows_for(2.0) == 100 + assert block.command == index and block.rows == rows_for(2.0) == 100 held = record.joints_rad[_span(record, block)] # Motor-step quantisation moves the pose by well under a hundredth of a # degree; what matters is that every row holds the same pose. @@ -53,7 +46,7 @@ def test_gripper_close_ramps_the_jaws_over_the_tools_travel(): block = record.blocks[index] expected = get_registry().get("SSG-48").estimate_duration("close", []) assert expected > 0 - assert block.rows == pytest.approx(_rows_for(expected), abs=1) + assert block.rows == pytest.approx(rows_for(expected), abs=1) closed = record.tool_closed[_span(record, block)] assert closed[0] == pytest.approx(0.0, abs=0.05) assert np.all(np.diff(closed) >= 0) and closed[-1] > 0.9 @@ -121,6 +114,6 @@ def test_budget_truncates_the_record_and_says_so(): full = client.plan() cut = client.plan(max_seconds=1.0) assert cut.stop == "budget_exhausted" - assert cut.rows == _rows_for(1.0) < full.rows + assert cut.rows == rows_for(1.0) < full.rows assert cut.blocks[0].rows == cut.rows and cut.blocks[1].rows == 0 assert full.stop == "completed" diff --git a/tests/unit/test_dry_run_script_compat.py b/tests/unit/test_dry_run_script_compat.py index 4872e84..eaba405 100644 --- a/tests/unit/test_dry_run_script_compat.py +++ b/tests/unit/test_dry_run_script_compat.py @@ -13,8 +13,8 @@ import pytest from waldoctl import CommandKind, command_table -from parol6.client.dry_run_client import _CMD_STRUCTS, _STRIDE, DryRunRobotClient -from parol6.config import INTERVAL_S +from parol6.client.dry_run_client import _CMD_STRUCTS, DryRunRobotClient +from tests.conftest import rows_for HOME = [90.0, -90.0, 180.0, 0.0, 0.0, 180.0] POSE_A = [0.0, 280.0, 200.0, 90.0, 0.0, 90.0] @@ -24,12 +24,6 @@ ANGLES_B = [70.0, -70.0, 200.0, 20.0, 20.0, 200.0] -def _rows_for(seconds: float) -> int: - import math - - return math.ceil(round(seconds / INTERVAL_S) / _STRIDE) - - @pytest.fixture def client(): return DryRunRobotClient() @@ -111,7 +105,7 @@ def test_flush(self, client): def test_delay(self, client): index = client.delay(1.0) - assert client.plan().blocks[index].rows == _rows_for(1.0) + assert client.plan().blocks[index].rows == rows_for(1.0) def test_wait_motion(self, client): client.move_j(ANGLES_A, speed=0.5) diff --git a/tests/unit/test_query_commands_actions.py b/tests/unit/test_query_commands_actions.py index ba24671..dc35d34 100644 --- a/tests/unit/test_query_commands_actions.py +++ b/tests/unit/test_query_commands_actions.py @@ -5,11 +5,10 @@ Uses minimal state objects to test command logic in isolation. """ -from types import SimpleNamespace - from waldoctl import ActionState from parol6.commands.query_commands import ActivityCommand +from parol6.server.state import ControllerState from parol6.protocol.wire import ( ActivityCmd, CurrentActionResultStruct, @@ -18,7 +17,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", @@ -38,7 +37,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="",