diff --git a/parol6/client/async_client.py b/parol6/client/async_client.py index 13886f4..6afb965 100644 --- a/parol6/client/async_client.py +++ b/parol6/client/async_client.py @@ -254,7 +254,7 @@ class AsyncRobotClient(_RobotClientABC): @property def skill_capabilities(self) -> frozenset[str]: - return super().skill_capabilities | {"backend.parol6"} + return super().skill_capabilities | {"backend.parol6", "tool.gripper"} def __init__( self, diff --git a/parol6/client/dry_run_client.py b/parol6/client/dry_run_client.py index 780bec6..1edba0d 100644 --- a/parol6/client/dry_run_client.py +++ b/parol6/client/dry_run_client.py @@ -146,6 +146,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( @@ -526,7 +542,9 @@ def _simulate_cartesian_jog(self, cmd: JogLCommand) -> DryRunResult | None: @property def skill_capabilities(self) -> frozenset[str]: - return frozenset({"motion.joint", "motion.linear", "backend.parol6"}) + return frozenset( + {"motion.joint", "motion.linear", "tool.gripper", "backend.parol6"} + ) def angles(self) -> list[float]: steps_to_rad(self._state.Position_in, self._q_rad_buf) diff --git a/parol6/config.py b/parol6/config.py index b738120..880a240 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..5ec66dc 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,11 +610,27 @@ 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 - msgs = self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT) + # Copied: the transport hands back a buffer it reuses on the next call. + msgs = list(self.udp_transport.poll_receive_all(max_count=MAX_POLL_COUNT)) + if len(msgs) == MAX_POLL_COUNT: + 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 + ) + msgs.extend(backlog) for data, addr in msgs: self._process_command(data, addr, state) @@ -701,10 +718,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/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" + )