Sends a single JPEG frame to the FR WebSocket server and returns the list of

detections.  The server runs SCRFD + ViT + ByteTrack; per-connection tracking

state resets when the connection closes, which is fine for the demo pattern of

        # [{"identity": "alice", "confidence": 0.92, "bbox": [x1, y1, x2, y2]}, ...]

    - **In:**  Raw binary JPEG/PNG bytes (one frame per message, any resolution)

    - **Out:** ``{"timestamp": ..., "detections": [{"identity": str|null,

               "confidence": float, "bbox": [x1, y1, x2, y2]}]}``

    A new WebSocket connection is opened per call.  This is adequate for the

    demo frequency of 1–2 calls per scene and avoids long-lived connection

    def __init__(self, config: Optional[FRConfig] = None) -> None:

    async def identify(self, jpeg_bytes: bytes) -> list[dict[str, Any]]:

        """Send a JPEG frame to the FR server and return detections.

            - ``identity``: str name or ``None`` if unrecognised

            Returns an empty list on any error (connection failure, timeout,

            logger.warning("FRClient.identify called with empty bytes — skipping")

            import websockets  # lazy import — optional dependency

                "websockets is not installed — cannot call FR server. "

                raw = await asyncio.wait_for(ws.recv(), timeout=self.config.timeout_s)

            detections: list[dict[str, Any]] = data.get("detections", [])

                "FR identified %d detection(s) from %d-byte frame",

This module provides the EdgeProxyClient for bidirectional WebSocket communication

with the robot Edge Proxy, following the Edge Proxy Design spec.

- Edge Proxy runs as a WebSocket *server* on the robot (default :8080/edge).

    This client connects to the robot's Edge Proxy server and handles

            print(f"Nav status: {msg.status}, progress: {msg.progress}")

        await client.send_navigate_waypoint("waypoint1", request_id="nav_001")

            config: Client configuration. Uses defaults if not provided.

            logger: Logger instance. Creates default logger if not provided.

        self._robot_state_handlers: List[RobotStateHandler] = []

        self._waypoint_list_handlers: List[WaypointListHandler] = []

        self._frame_response_handlers: List[FrameResponseHandler] = []

        self._fr_detections_handlers: List[FRDetectionsHandler] = []

        self._connection_change_handlers: List[ConnectionChangeHandler] = []

        self._receiver_task: Optional[asyncio.Task[None]] = None

        # Last received robot state (updated on every robot_state message)

        # Last received FR detections (updated on every fr_detections message)

        self.last_fr_detections: Optional[FRDetectionsMessage] = None

        # Populated by send_capture_frame(); resolved by _dispatch_message().

    def on_nav_status(self, handler: NavStatusHandler) -> NavStatusHandler:

            handler: Async or sync function that takes NavStatusMessage.

    def on_robot_state(self, handler: RobotStateHandler) -> RobotStateHandler:

            handler: Async or sync function that takes RobotState.

    def on_waypoint_list(self, handler: WaypointListHandler) -> WaypointListHandler:

            handler: Async or sync function that takes WaypointListMessage.

            handler: Async or sync function that takes ErrorMessage.

            handler: Async or sync function that takes PongMessage.

    def on_frame_response(self, handler: FrameResponseHandler) -> FrameResponseHandler:

        Called when the Edge Proxy sends a ``frame_response`` message.

        Note: futures registered via ``send_capture_frame`` are resolved

        *before* these handlers are called, so handlers see all responses

            handler: Async or sync function that takes FrameResponseMessage.

    def on_connection_change(self, handler: ConnectionChangeHandler) -> ConnectionChangeHandler:

            handler: Async or sync function that takes bool (connected).

        return self._connected and self._ws is not None and not self._ws.closed

        If reconnect is enabled in config, will automatically reconnect on disconnect.

            EdgeProxyConnectionError: If initial connection fails and reconnect is disabled.

        Stops the receiver task and closes the WebSocket connection.

        cmd = NavigateCommand.to_waypoint(name, request_id, speed)

        cmd = NavigateCommand.to_pose(x, y, theta, request_id, speed)

            direction: Direction ("forward", "backward", "left", "right").

            speed: Navigation speed (default "slow" for relative).

        cmd = NavigateCommand.to_relative(direction, distance, request_id, speed)

    async def send_get_state(self, request_id: str = "") -> None:

        The response will be delivered via on_robot_state handlers.

        cmd = GetStateCommand(type=MessageType.GET_STATE, request_id=request_id)

    async def send_capture_frame(self, request_id: str = "") -> None:

        The Edge Proxy will capture one JPEG frame from the camera and reply

        with a ``frame_response`` message.  To await the response, create an

        ``asyncio.Future`` and store it in ``self._pending_frames[request_id]``

        *before* calling this method, then ``await`` the future.

        In practice callers should use ``ActionExecutor._capture_frame()``

            request_id: Correlation ID used to match the response.

            except (OSError, websockets.exceptions.WebSocketException) as exc:

                self.logger.warning("Connection failed: %s", exc)

                if not self.config.reconnect or self._should_stop:

                    raise EdgeProxyConnectionError(f"Failed to connect: {exc}") from exc

                # Exponential backoff: 1s, 2s, 4s, 8s... max 30s

                await asyncio.sleep(min(delay, self.config.max_reconnect_delay))

        Endpoint: ws://<robot-ip>:8080/edge (defaults; configurable)

        uri = f"ws://{self.config.host}:{self.config.port}{self.config.ws_path}"

        self._receiver_task = asyncio.create_task(self._receive_messages())

            self.logger.error("Error receiving message: %s", exc)

                message = message.decode("utf-8", errors="replace")

                self.logger.error("Failed to decode message: %s", exc)

            self.logger.error("Unsupported message type: %s", type(message))

            self.logger.error("Failed to parse message: %s", exc)

            self.logger.debug("Unknown message type: %s", data.get("type"))

            await self._call_handlers(self._nav_status_handlers, msg)

            await self._call_handlers(self._robot_state_handlers, msg)

            await self._call_handlers(self._waypoint_list_handlers, msg)

            await self._call_handlers(self._error_handlers, msg)

            # Resolve a pending Future if one is registered for this request_id

            future = self._pending_frames.pop(msg.request_id, None)

            await self._call_handlers(self._frame_response_handlers, msg)

    async def _call_handlers(self, handlers: List[Any], msg: Any) -> None:

                    "Handler %s failed: %s", handler.__name__, exc

    async def _send_message(self, data: Dict[str, Any]) -> None:

            raise EdgeProxyConnectionError("Not connected to Edge Proxy")

            self.logger.error("Failed to send message: %s", exc)

            raise EdgeProxyConnectionError(f"Failed to send message: {exc}") from exc

    async def _notify_connection_change(self, connected: bool) -> None:

from executor.action_executor import ActionExecutor, ExecutionReport

from planner.planner import PlanContext, PlanRequest, PlannerService

from state_machine import InvalidTransition, OrchestratorStateMachine, OrchestratorState, TransitionRecord

    llm_endpoint: str = "https://modelapi.klass.dev/v1/chat/completions"

        state_machine: Optional[OrchestratorStateMachine] = None,

        self.state_machine = state_machine or OrchestratorStateMachine()

        self.planner = planner if planner is not None else self._build_default_planner()

        self.executor = executor if executor is not None else (ActionExecutor() if config.enable_executor else None)

        self._edge_client_connect_task: Optional[asyncio.Task[None]] = None

        # FR temporal analyzer — watches detection stream for consistent faces

        # Dashboard WebSocket connections for confirmation broadcasts

        self._dashboard_clients: Set[WebSocketServerProtocol] = set()

            self.executor.register_broadcast_callback(self.broadcast_to_dashboard)

    def _build_default_planner(self) -> Optional[PlannerService]:

            include_response_format=self.config.llm_include_response_format,

        return PlannerService(llm_client=LLMClient(config=llm_config))

        """Set up state machine listener to trigger executor on EXECUTING_ACTIONS."""

        def on_state_transition(record: TransitionRecord) -> None:

            if record.next == OrchestratorState.EXECUTING_ACTIONS:

                    "State transitioned to EXECUTING_ACTIONS, triggering executor"

                    asyncio.create_task(self._execute_plan_and_transition(plan))

                    self.logger.warning("No plan to execute, transitioning to SPEAKING")

                    # No plan to execute, immediately transition to SPEAKING

                    asyncio.create_task(self._send_actions_complete())

        """Set up state machine listener to broadcast plans on AWAITING_CONFIRM."""

            if record.next == OrchestratorState.AWAITING_CONFIRM:

                    "State transitioned to AWAITING_CONFIRM, broadcasting plan to dashboard"

                # Broadcast plan to all connected dashboard clients

                asyncio.create_task(self._broadcast_confirmation_request(record))

            self.logger.warning("Edge Proxy enabled but executor not available")

            self._edge_client = EdgeProxyClient(config=edge_config, logger=self.logger)

            self.executor.register_edge_proxy_client(self._edge_client)

            # Wire FR temporal analyzer to the edge proxy detection stream.

            self._fr_analyzer.on_consistent_detection(self._on_consistent_fr_detection)

            self._edge_client.on_fr_detections(self._on_fr_detections)

            self.logger.error("Failed to set up Edge Proxy client: %s", exc)

    async def _execute_plan_and_transition(self, plan: Dict[str, Any]) -> None:

            self.logger.error("Executor not available for plan execution")

            await self._send_actions_failed("executor_not_available")

        def _on_replan_requested(signal: Dict[str, Any]) -> None:

                    "Dynamic replanning requested at step=%s reasons=%s",

                self.executor.cancel("dynamic_replan_requested")

            self.logger.info("Executing plan: %s", plan.get("plan_id", "unknown"))

            # If scan triggered replan and planner is enabled, refine and continue.

            if replan_signal is not None and self.planner is not None:

                            "previous_plan_id": plan.get("plan_id"),

                        refined_graph.plan.get("plan_id", "unknown"),

                    report = await self.executor.execute_plan(refined_graph.plan)

                    self.logger.error("Dynamic replan failed: %s", exc)

                    await self._send_actions_failed(f"dynamic_replan_failed: {exc}")

                self.logger.info("Plan execution completed successfully")

                self.logger.warning("Plan execution failed: %s", report.errors)

                await self._send_actions_failed(report.errors[0] if report.errors else "unknown_error")

            self.logger.warning("Invalid transition on actions_complete: %s", exc)

            self.state_machine.handle_event("actions_failed", {"error": error})

            self.logger.warning("Invalid transition on actions_failed: %s", exc)

    async def _broadcast_confirmation_request(self, record: TransitionRecord) -> None:

        """Broadcast plan confirmation request to all connected dashboard clients.

            record: Transition record containing the plan in its payload.

            self.logger.warning("No dashboard clients connected to send confirmation request")

            self.logger.warning("No plan available for confirmation request")

                self.logger.debug("Sent confirmation request to dashboard client %s", client.remote_address)

                self.logger.warning("Failed to send confirmation request to %s: %s", client.remote_address, exc)

            self.logger.info("Removed %d disconnected dashboard clients", len(disconnected))

    async def broadcast_to_dashboard(self, message: Dict[str, Any]) -> None:

        """Broadcast a message to all connected dashboard WebSocket clients.

        Serializes the message to JSON and sends it to every client in

        ``_dashboard_clients``. Removes any clients that fail to receive.

            self.logger.debug("broadcast_to_dashboard: no clients connected")

                    "broadcast_to_dashboard: failed to send to %s: %s",

                "broadcast_to_dashboard: removed %d disconnected clients",

            "Starting orchestrator server on %s:%s", self.config.host, self.config.port

        if self._edge_client is not None and self._edge_client_connect_task is None:

            # Start connecting in the background so server startup isn't blocked.

            self._edge_client_connect_task = asyncio.create_task(self._edge_client.connect())

            self._edge_client_connect_task.add_done_callback(self._on_edge_client_connect_done)

                    self.logger.warning("Edge Proxy disconnect failed: %s", exc)

            if self._edge_client_connect_task is not None and not self._edge_client_connect_task.done():

                    self.logger.warning("Edge Proxy connect task error during shutdown: %s", exc)

    def _on_edge_client_connect_done(self, task: asyncio.Task[None]) -> None:

            # If reconnect is enabled (default), connect() usually won't raise.

            self.logger.error("Edge Proxy connect task failed: %s", exc)

    async def _handler(self, websocket: WebSocketServerProtocol, path: str) -> None:

            self.logger.info("Client connected from %s", websocket.remote_address)

                    response = await self._handle_message(message)

                        await websocket.send(json.dumps(response))

                self.logger.warning("Connection error: %s", exc)

                self.logger.info("Client disconnected from %s", websocket.remote_address)

            self.logger.info("Dashboard client connected from %s", websocket.remote_address)

                    response = await self._handle_dashboard_message(message)

                self.logger.warning("Dashboard connection error: %s", exc)

                self.logger.info("Dashboard client disconnected from %s", websocket.remote_address)

        await websocket.close(code=1008, reason="Unsupported path")

    async def _handle_message(self, message: Any) -> Optional[Dict[str, Any]]:

                return {"type": "error", "error": "invalid_bytes"}

            return {"type": "error", "error": "unsupported_message"}

            return {"type": "error", "error": "invalid_payload"}

            return {"type": "pong", "state": self.state_machine.state.value}

                return self._advance_state("final_transcript", payload)

            return {"type": "ack", "state": self.state_machine.state.value}

                return {"type": "error", "error": "planner_disabled"}

                return {"type": "error", "error": "missing_intent"}

                request_id=payload.get("request_id", "request"),

            result = await asyncio.to_thread(self.planner.plan, request)

            return {"type": "plan_result", **_plan_result_payload(result)}

                return {"type": "error", "error": "executor_disabled"}

                return {"type": "error", "error": "missing_plan"}

            report = await self.executor.execute_plan(plan, context=payload.get("context"))

            return {"type": "execution_report", **_report_payload(report)}

            return self._advance_state("user_confirmed", {"plan": plan})

            return self._advance_state("user_rejected", payload)

        return {"type": "error", "error": "unknown_message", "detail": msg_type}

    async def _handle_dashboard_message(self, message: Any) -> Optional[Dict[str, Any]]:

        Dashboard clients can send confirmation responses (user_confirmed/user_rejected)

    def _advance_state(self, event: str, payload: Dict[str, Any]) -> Dict[str, Any]:

            new_state = self.state_machine.handle_event(event, payload)

        return {"type": "state_update", "event": event, "state": new_state.value}

        if path not in ("/", self.config.ws_path, self.config.dashboard_ws_path):

def _build_context(payload: Optional[Dict[str, Any]]) -> PlanContext:

        current_location=payload.get("current_location", "unknown"),

        additional_constraints=payload.get("additional_constraints", {}),

    parser = argparse.ArgumentParser(description="Orchestrator WebSocket server")

    parser.add_argument("--host", default=os.getenv("ORCHESTRATOR_HOST", "0.0.0.0"))

        default=os.getenv("ORCHESTRATOR_DASHBOARD_WS_PATH", "/dashboard"),

        default=_env_flag("ORCHESTRATOR_ENABLE_PLANNER", False),

        default=_env_flag("ORCHESTRATOR_DISABLE_EXECUTOR", False),

        default=_env_flag("ORCHESTRATOR_ENABLE_EDGE_PROXY", False),

        default=os.getenv("ORCHESTRATOR_EDGE_PROXY_HOST", "localhost"),

        default=int(os.getenv("ORCHESTRATOR_EDGE_PROXY_PORT", "8080")),

        default=os.getenv("ORCHESTRATOR_LLM_ENDPOINT", "https://modelapi.klass.dev/v1/chat/completions"),

        default=os.getenv("ORCHESTRATOR_LLM_MODEL", "Qwen3-Next-80B-A3B-FP8"),

        default=int(os.getenv("ORCHESTRATOR_LLM_TIMEOUT_S", "30")),

        default=_env_flag("ORCHESTRATOR_LLM_NO_RESPONSE_FORMAT", False),

        default=int(os.getenv("ORCHESTRATOR_LLM_MAX_TOKENS")) if os.getenv("ORCHESTRATOR_LLM_MAX_TOKENS") else None,

        default=float(os.getenv("ORCHESTRATOR_LLM_TOP_P")) if os.getenv("ORCHESTRATOR_LLM_TOP_P") else None,

        default=int(os.getenv("ORCHESTRATOR_LLM_TOP_K")) if os.getenv("ORCHESTRATOR_LLM_TOP_K") else None,

        default=_env_flag("ORCHESTRATOR_LLM_USE_OPENAI_SDK", False),

        llm_include_response_format=not args.llm_no_response_format,

This module defines the message types and dataclasses for communication

with the robot Edge Proxy over WebSocket, following the Edge Proxy Design spec.

    These correspond to the message types defined in the Edge Proxy Design spec.

    goal: Dict[str, Any] = None  # WaypointGoal, PoseGoal, or RelativeGoal as dict

    def to_waypoint(cls, name: str, request_id: str = "", speed: str = Speed.NORMAL) -> NavigateCommand:

                request_id: str = "", speed: str = Speed.NORMAL) -> NavigateCommand:

            goal={"type": GoalType.POSE, "x": x, "y": y, "theta": theta},

                   request_id: str = "", speed: str = Speed.SLOW) -> NavigateCommand:

            goal={"type": GoalType.RELATIVE, "direction": direction, "distance": distance},

    The Edge Proxy will respond with a FrameResponseMessage containing the

        return {"type": self.type, "request_id": self.request_id}

    """Navigation status update message (Edge Proxy -> Orchestrator).

    def from_dict(cls, data: Dict[str, Any]) -> NavStatusMessage:

    def from_dict(cls, data: Dict[str, Any]) -> WaypointListMessage:

        waypoints = [Waypoint.from_dict(wp) for wp in waypoints_data]

    """Frame response from Edge Proxy containing a captured JPEG or an error.

    Sent in response to a CaptureFrameCommand.  Either ``jpeg_b64`` or

    def from_dict(cls, data: Dict[str, Any]) -> "FrameResponseMessage":

IncomingMessage = Union[NavStatusMessage, RobotState, WaypointListMessage, ErrorMessage, PongMessage, FrameResponseMessage]

def parse_edge_message(data: Dict[str, Any]) -> Optional[IncomingMessage]:

        The appropriate message dataclass or None if the message type is unknown.

        ValueError: If message data is invalid (missing type field).

All websockets calls are mocked so these tests run without websockets installed.

# ── path setup ──────────────────────────────────────────────────────────────

# ── inject a fake websockets before importing the client ────────────────────

# websockets may not be installed in the dev environment; we stub the entire

# module so FRClient can be imported and tested without the real dependency.

    """Build a minimal websockets stub sufficient for FRClient tests."""

from services.fr_client import FRClient, FRConfig  # noqa: E402  (after sys.modules patch)

# ── constants ────────────────────────────────────────────────────────────────

_FAKE_JPEG = b"\xff\xd8\xff" + b"\x00" * 64  # minimal JPEG-like bytes

# ── helpers ───────────────────────────────────────────────────────────────────

    """Build a mock async context manager that looks like a websockets connection.

        A MagicMock usable as ``async with websockets.connect(...) as ws:``.

# ── tests ────────────────────────────────────────────────────────────────────

    def test_endpoint_from_env(self, monkeypatch: pytest.MonkeyPatch) -> None:

        config = FRConfig(endpoint="ws://custom:1234/", timeout_s=30)

    async def test_identify_success_returns_detections(self) -> None:

        """Valid JSON response → identify() returns detection list."""

                    {"identity": "alice", "confidence": 0.92, "bbox": [10, 20, 100, 200]},

                    {"identity": None, "confidence": 0.45, "bbox": [300, 50, 400, 180]},

        with patch.object(websockets, "connect", return_value=ctx):

        """identify() must send the raw bytes to the WebSocket."""

    async def test_identify_empty_detections_response(self) -> None:

        """Response with empty detections list → returns empty list."""

        payload = json.dumps({"timestamp": 0.0, "detections": []})

    async def test_identify_connection_error_returns_empty(self) -> None:

        """Connection failure → identify() returns [] (no raise)."""

            websockets, "connect", side_effect=ConnectionRefusedError("refused")

    async def test_identify_timeout_returns_empty(self) -> None:

        """asyncio.TimeoutError on recv → identify() returns []."""

    async def test_identify_malformed_json_returns_empty(self) -> None:

    async def test_identify_empty_bytes_returns_empty_without_connecting(

        """Empty bytes are rejected before opening a connection."""

        with patch.object(websockets, "connect") as mock_connect:

        with patch.object(websockets, "connect", return_value=ctx) as mock_connect:

"""FR Temporal Analyzer — watches face recognition detections for patterns.

Accumulates face recognition detections over a sliding window and identifies

when a face is consistently detected (same identity or 'unknown') for a

configurable threshold duration.  When a consistent detection is found,

triggers an async callback so the orchestrator can reason about it.

from typing import Any, Callable, Coroutine, Dict, List, Optional, Set, Tuple

# Callback signature: async fn(identity: str, confidence: float, duration: float)

    Accumulates detections over a sliding window and identifies when

    a face is consistently detected (same identity or ``'unknown'``) for

    a configurable threshold duration.  When a consistent detection is

    found, triggers a callback so the orchestrator can reason about it.

    # Minimum fraction of frames in the threshold window an identity must

    # Minimum number of frames required before any consistency check.

        self._detection_history: List[Tuple[float, List[Dict[str, Any]]]] = []

        self._on_consistent_detection: Optional[ConsistentDetectionCallback] = None

        """Register callback: ``async fn(identity, confidence, duration)``."""

        Called on every ``fr_detections`` message from the edge proxy.

            detections: List of detection dicts (each with at least

            timestamp: Unix epoch seconds of the detection frame.

            (t, d) for t, d in self._detection_history if t > cutoff

        """Reset all state.  Call when the robot moves to a new location."""

        """Allow re-notification for *identity* (after it has been handled)."""

        """Find identities present consistently for ≥ threshold seconds.

            List of ``(identity, avg_confidence, duration_seconds)``.

            (t, d) for t, d in self._detection_history if t >= window_start

        identity_frames: Dict[str, List[Tuple[float, float]]] = {}

                identity_frames.setdefault(ident, []).append((ts, conf))

            if len(frames) / total_frames >= self.PRESENCE_RATIO:

                avg_conf = sum(c for _, c in frames) / len(frames)

                    results.append((identity, avg_conf, duration))