diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 3c17cc0..fbf0f42 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -149,6 +149,22 @@ class _DryRunTool: def __init__(self, client: DryRunRobotClient) -> None: self._client = client + @property + def key(self) -> str: + return self._client._active_tool_key + + @property + def tool_type(self) -> str: + from waldoctl.tools import ToolType + from parol6.tools import ElectricGripperConfig, PneumaticGripperConfig + + spec = get_registry().get(self.key) + return ( + ToolType.GRIPPER + if isinstance(spec, (ElectricGripperConfig, PneumaticGripperConfig)) + else ToolType.NONE + ) + def __getattr__(self, name: str) -> Any: def method(*args: Any, **kwargs: Any) -> DryRunResult | None: return self._client.tool_action( diff --git a/parol6/config.py b/parol6/config.py index 2f22351..80ebf96 100644 --- a/parol6/config.py +++ b/parol6/config.py @@ -22,6 +22,11 @@ MAX_COMMAND_QUEUE_SIZE: int = 100 MAX_BLEND_LOOKAHEAD: int = int(os.getenv("PAROL6_MAX_BLEND_LOOKAHEAD", "100")) MAX_POLL_COUNT: int = 25 # Max UDP messages to read per control tick +# Further messages read in a tick whose batch filled up. A client streaming +# faster than the tick leaves a backlog in the socket; it is already stale, so +# carrying it to later ticks makes the arm chase old targets and delays the +# stop behind them by as many ticks as the backlog is deep. +MAX_BACKLOG_COUNT: int = int(os.getenv("PAROL6_MAX_BACKLOG_COUNT", "500")) # Serial transport defaults SERIAL_RX_RING_DEFAULT: int = 262144 diff --git a/parol6/server/controller.py b/parol6/server/controller.py index 25d06b0..0282cb8 100644 --- a/parol6/server/controller.py +++ b/parol6/server/controller.py @@ -66,6 +66,7 @@ from parol6.config import ( TRACE, INTERVAL_S, + MAX_BACKLOG_COUNT, MAX_POLL_COUNT, MCAST_GROUP, MCAST_PORT, @@ -609,13 +610,30 @@ def _main_control_loop(self): state.Speed_out.fill(0) def _poll_commands(self, state: ControllerState) -> None: - """Poll and process UDP commands (non-blocking).""" + """Poll and process UDP commands (non-blocking). + + A full batch means a client outran the tick, so the rest of the socket + is read in this tick as well: each streaming command supersedes the one + before it, so the arm ends the tick on the newest target instead of + following a queue of old ones for as many ticks as the backlog is deep, + and the configuration, queries and stops mixed into it are still seen, + in order -- which a blind socket drain threw away. + """ assert self.udp_transport is not None state.command_out_locked = False + # The transport reuses its batch buffer, so each batch is processed + # before the next one is read; nothing is copied on the tick. msgs = self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT) + full = len(msgs) == MAX_POLL_COUNT for data, addr in msgs: self._process_command(data, addr, state) + if full: + backlog = self.udp_transport.poll_receive_all(max_count=MAX_BACKLOG_COUNT) + if len(backlog) == MAX_BACKLOG_COUNT: + logger.log(TRACE, "udp_backlog_capped count=%d", MAX_BACKLOG_COUNT) + for data, addr in backlog: + self._process_command(data, addr, state) def _reply_error(self, addr: tuple[str, int], error: RobotError) -> None: """Send error response to client. Caller must ensure udp_transport is not None.""" @@ -701,10 +719,8 @@ def _handle_motion_command( self._segment_player.cancel(state) # Unconditional: a jog self-collision sets the viz but no state.error. state.clear_collision() - if self.udp_transport: - drained = self.udp_transport.drain_buffer() - if drained > 0: - logger.log(TRACE, "udp_buffer_drained count=%d", drained) + # Coalesce decoded motion only: unread UDP packets can contain + # configuration, queries, or stop commands that must survive. self._executor.cancel_active_streamable() removed = self._executor.clear_streamable_commands( "Streaming command prepare" diff --git a/parol6/server/transports/udp_transport.py b/parol6/server/transports/udp_transport.py index 7c2e571..e36752b 100644 --- a/parol6/server/transports/udp_transport.py +++ b/parol6/server/transports/udp_transport.py @@ -140,32 +140,6 @@ def poll_receive_all( self._recv_all_buf.append(msg) return self._recv_all_buf - def drain_buffer(self) -> int: - """ - Drain all pending messages from the UDP receive buffer. - - This is useful in stream mode to discard stale commands when new ones arrive. - Returns the number of messages drained. - """ - if not self.socket or not self._running: - return 0 - - drained_count = 0 - try: - # Socket is already non-blocking; read all pending messages - while True: - try: - nbytes, _ = self.socket.recvfrom_into(self._rxv) - if nbytes > 0: - drained_count += 1 - except (BlockingIOError, OSError): - # No more data available (expected) - break - except Exception as e: - logger.debug(f"Error draining UDP buffer: {e}") - - return drained_count - def send(self, data: bytes, address: tuple[str, int]) -> bool: """ Send raw bytes to a specific address. diff --git a/tests/unit/test_reset_enable_reaches_firmware.py b/tests/unit/test_reset_enable_reaches_firmware.py index e73be64..973f272 100644 --- a/tests/unit/test_reset_enable_reaches_firmware.py +++ b/tests/unit/test_reset_enable_reaches_firmware.py @@ -76,3 +76,58 @@ def tick_until(condition, message: str) -> None: "RESET's ENABLE never reached the firmware write phase — " f"written command codes: {sorted(set(written))}" ) + + +def test_streaming_packet_keeps_following_configuration_and_stop(controller): + from parol6.config import MAX_POLL_COUNT + from parol6.protocol.wire import PingCmd, SelectProfileCmd, TeleportCmd + + state = controller.state_manager.get_state() + assert controller.udp_transport is not None + address = ("127.0.0.1", controller.udp_transport.socket.getsockname()[1]) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender: + sender.sendto( + encode_command(TeleportCmd(angles=[90, -90, 180, 0, 0, 180])), address + ) + for _ in range(MAX_POLL_COUNT - 1): + sender.sendto(encode_command(PingCmd()), address) + sender.sendto(encode_command(SelectProfileCmd(profile="QUINTIC")), address) + sender.sendto(encode_command(EstopCmd()), address) + for _ in range(2): + controller._poll_commands(state) + controller._execute_commands(state) + + assert state.motion_profile == "QUINTIC", ( + "streaming must not discard pending configuration" + ) + assert not state.enabled, "a queued stop must survive a streaming batch boundary" + + +def test_a_streaming_flood_is_consumed_in_the_tick_it_arrived_in(controller): + """A client streaming faster than one tick's batch leaves a backlog in the + socket. It is stale the moment the tick runs, so the tick has to consume + it: otherwise the arm follows superseded targets for backlog/batch ticks + and anything queued behind them — here a stop — waits just as long.""" + from parol6.config import MAX_POLL_COUNT + from parol6.protocol.wire import JogJCmd + + state = controller.state_manager.get_state() + assert controller.udp_transport is not None + address = ("127.0.0.1", controller.udp_transport.socket.getsockname()[1]) + flood = MAX_POLL_COUNT * 4 + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender: + for _ in range(flood): + sender.sendto( + encode_command( + JogJCmd(speeds=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], duration=0.2) + ), + address, + ) + sender.sendto(encode_command(EstopCmd()), address) + controller._poll_commands(state) + controller._execute_commands(state) + + assert not state.enabled, ( + f"one tick left {flood} streamed targets unread, so the stop behind " + f"them was not seen either" + )