523 lines
22 KiB
Python
523 lines
22 KiB
Python
"""Async WebSocket server for local gesture events."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import asyncio
|
|
import json
|
|
import sys
|
|
from collections.abc import Iterable
|
|
from contextlib import suppress
|
|
from dataclasses import replace
|
|
from typing import Any, get_args
|
|
|
|
from .cameras import (
|
|
NullCameraInput,
|
|
UrlCameraInput,
|
|
UrlCameraSpec,
|
|
UsbCameraInput,
|
|
UsbCameraSpec,
|
|
)
|
|
from .config import MotionAgentConfig
|
|
from .events import (
|
|
CommandResultEvent,
|
|
GestureEvent,
|
|
GestureName,
|
|
HeartbeatEvent,
|
|
SkeletonEvent,
|
|
StatusEvent,
|
|
)
|
|
from .recognizer import GestureObservation, GestureRecognizer, NullGestureRecognizer
|
|
from .state import GestureStateMachine
|
|
|
|
|
|
ALLOWED_GESTURES = set(get_args(GestureName))
|
|
|
|
|
|
class MotionAgentServer:
|
|
def __init__(
|
|
self,
|
|
config: MotionAgentConfig,
|
|
cameras: Iterable[Any] | None = None,
|
|
recognizer: GestureRecognizer | None = None,
|
|
) -> None:
|
|
self.config = config
|
|
self.cameras = list(cameras) if cameras is not None else self._build_cameras(config)
|
|
self.recognizer = recognizer or (NullGestureRecognizer() if config.dry_run else None)
|
|
self.recognizer_name = recognizer.name if recognizer is not None else (
|
|
"dry-run" if config.dry_run else "mediapipe-opencv"
|
|
)
|
|
self.state = GestureStateMachine(
|
|
confidence_threshold=config.confidence_threshold,
|
|
cooldown_ms=config.cooldown_ms,
|
|
mode=self._resolve_mode(),
|
|
)
|
|
self.clients: set[Any] = set()
|
|
self.last_gesture: str | None = None
|
|
self.last_error: str | None = None
|
|
self.last_fusion_reason: str | None = None
|
|
self.fps = 0.0
|
|
self.recognition_fps = 0.0
|
|
self.armed = False
|
|
self.paused = False
|
|
self.devices_open = False
|
|
self.debug_skeleton_enabled = False
|
|
self.enabled_gestures: set[str] = set(ALLOWED_GESTURES)
|
|
self.fusion_window_ms = config.fusion_window_ms
|
|
self.fusion_conflict_delta = config.fusion_conflict_delta
|
|
self._stop = asyncio.Event()
|
|
self._recognition_subprocess: asyncio.subprocess.Process | None = None
|
|
|
|
def _build_cameras(self, config: MotionAgentConfig) -> list[Any]:
|
|
if config.dry_run:
|
|
return [NullCameraInput()]
|
|
if config.camera_urls:
|
|
return [
|
|
UrlCameraInput(UrlCameraSpec(url=url, camera_id=f"url:{index}"))
|
|
for index, url in enumerate(config.camera_urls)
|
|
]
|
|
specs = [
|
|
UsbCameraSpec(
|
|
index=index,
|
|
width=config.camera_width,
|
|
height=config.camera_height,
|
|
fps=config.camera_fps,
|
|
)
|
|
for index in config.camera_indexes
|
|
]
|
|
return [UsbCameraInput(spec) for spec in specs]
|
|
|
|
def _resolve_mode(self) -> str:
|
|
mode = self.config.mode
|
|
if mode == "dual":
|
|
return "dual_redundant"
|
|
if mode in {"single", "dual_redundant", "single_fallback", "calibrated_3d"}:
|
|
return mode
|
|
return "dual_redundant" if len(self.cameras) >= 2 else "single"
|
|
|
|
def close_cameras(self) -> None:
|
|
for camera in self.cameras:
|
|
with suppress(Exception):
|
|
camera.close()
|
|
self.devices_open = False
|
|
|
|
def rebuild_cameras(
|
|
self,
|
|
*,
|
|
camera_indexes: Iterable[int] | None = None,
|
|
camera_urls: Iterable[str] | None = None,
|
|
width: int | None = None,
|
|
height: int | None = None,
|
|
fps: int | None = None,
|
|
) -> None:
|
|
self.close_cameras()
|
|
next_config = replace(
|
|
self.config,
|
|
camera_indexes=tuple(camera_indexes) if camera_indexes is not None else self.config.camera_indexes,
|
|
camera_urls=tuple(camera_urls) if camera_urls is not None else self.config.camera_urls,
|
|
camera_width=width or self.config.camera_width,
|
|
camera_height=height or self.config.camera_height,
|
|
camera_fps=fps or self.config.camera_fps,
|
|
)
|
|
self.config = next_config
|
|
self.cameras = self._build_cameras(next_config)
|
|
|
|
def status_event(self, connected: bool = True) -> StatusEvent:
|
|
return StatusEvent(
|
|
connected=connected,
|
|
camera_count=len(self.cameras),
|
|
active_camera_ids=tuple(camera.camera_id for camera in self.cameras),
|
|
mode=self.state.mode,
|
|
input_mode=self.state.mode,
|
|
armed=self.armed,
|
|
paused=self.paused,
|
|
devices_open=self.devices_open,
|
|
recognizer=self.recognizer_name,
|
|
fps=self.fps,
|
|
recognition_fps=self.recognition_fps,
|
|
last_gesture=self.last_gesture,
|
|
last_fusion_reason=self.last_fusion_reason,
|
|
enabled_gestures=tuple(sorted(self.enabled_gestures)),
|
|
error=self.last_error,
|
|
)
|
|
|
|
def status_payload(self) -> dict[str, Any]:
|
|
return self.status_event().to_dict()
|
|
|
|
async def broadcast(
|
|
self,
|
|
event: CommandResultEvent | GestureEvent | HeartbeatEvent | SkeletonEvent | StatusEvent,
|
|
) -> None:
|
|
if not self.clients:
|
|
return
|
|
payload = event.to_json()
|
|
stale = []
|
|
for websocket in self.clients:
|
|
try:
|
|
await websocket.send(payload)
|
|
except Exception:
|
|
stale.append(websocket)
|
|
for websocket in stale:
|
|
self.clients.discard(websocket)
|
|
|
|
async def broadcast_payload(self, payload: dict[str, Any]) -> None:
|
|
if not self.clients:
|
|
return
|
|
message = json.dumps(payload, ensure_ascii=False, separators=(",", ":"))
|
|
stale = []
|
|
for websocket in self.clients:
|
|
try:
|
|
await websocket.send(message)
|
|
except Exception:
|
|
stale.append(websocket)
|
|
for websocket in stale:
|
|
self.clients.discard(websocket)
|
|
|
|
def _parse_command(self, raw_message: Any) -> tuple[str | None, str | None, dict[str, Any]]:
|
|
try:
|
|
data = json.loads(raw_message) if isinstance(raw_message, str) else raw_message
|
|
except json.JSONDecodeError:
|
|
return None, None, {}
|
|
if not isinstance(data, dict) or data.get("type") != "command":
|
|
return None, None, {}
|
|
command = str(data.get("command") or "").strip()
|
|
request_id = data.get("request_id")
|
|
payload = data.get("payload")
|
|
return command, str(request_id) if request_id is not None else None, payload if isinstance(payload, dict) else {}
|
|
|
|
async def handle_command(self, raw_message: Any) -> CommandResultEvent:
|
|
command, request_id, payload = self._parse_command(raw_message)
|
|
if not command:
|
|
return CommandResultEvent(
|
|
command="unknown",
|
|
request_id=request_id,
|
|
ok=False,
|
|
status=self.status_payload(),
|
|
error="Expected a JSON command message.",
|
|
)
|
|
|
|
try:
|
|
if command == "open_devices":
|
|
self._apply_device_payload(payload)
|
|
await self.restart_recognition_subprocess()
|
|
elif command == "close_devices":
|
|
await self.stop_recognition_subprocess()
|
|
self.devices_open = False
|
|
await self.broadcast(self.status_event(connected=True))
|
|
elif command == "rescan_devices":
|
|
self._apply_device_payload(payload, rebuild_only=True)
|
|
await self.restart_recognition_subprocess()
|
|
elif command == "set_armed":
|
|
self.armed = bool(payload.get("armed", True))
|
|
elif command == "set_paused":
|
|
self.paused = bool(payload.get("paused", True))
|
|
elif command == "set_input_mode":
|
|
self._set_input_mode(str(payload.get("input_mode") or payload.get("mode") or "auto"))
|
|
await self.restart_recognition_subprocess()
|
|
elif command == "set_camera_config":
|
|
self._apply_device_payload(payload, rebuild_only=not self.devices_open)
|
|
await self.restart_recognition_subprocess()
|
|
elif command == "set_fusion_config":
|
|
self.fusion_window_ms = int(payload.get("fusion_window_ms", self.fusion_window_ms))
|
|
self.fusion_conflict_delta = float(
|
|
payload.get("fusion_conflict_delta", self.fusion_conflict_delta)
|
|
)
|
|
elif command == "set_debug_options":
|
|
if "skeleton" in payload:
|
|
next_enabled = bool(payload["skeleton"])
|
|
if self.debug_skeleton_enabled != next_enabled:
|
|
self.debug_skeleton_enabled = next_enabled
|
|
await self.restart_recognition_subprocess()
|
|
elif command == "set_enabled_gestures":
|
|
gestures = payload.get("gestures")
|
|
if not isinstance(gestures, list):
|
|
raise ValueError("set_enabled_gestures requires payload.gestures list.")
|
|
next_gestures = {str(gesture) for gesture in gestures if str(gesture) in ALLOWED_GESTURES}
|
|
self.enabled_gestures = next_gestures or set(ALLOWED_GESTURES)
|
|
elif command in {"get_status", "ping"}:
|
|
pass
|
|
else:
|
|
return CommandResultEvent(
|
|
command=command,
|
|
request_id=request_id,
|
|
ok=False,
|
|
status=self.status_payload(),
|
|
error=f"Unsupported command: {command}",
|
|
)
|
|
return CommandResultEvent(
|
|
command=command,
|
|
request_id=request_id,
|
|
ok=True,
|
|
status=self.status_payload(),
|
|
)
|
|
except Exception as exc:
|
|
self.last_error = str(exc)
|
|
return CommandResultEvent(
|
|
command=command,
|
|
request_id=request_id,
|
|
ok=False,
|
|
status=self.status_payload(),
|
|
error=str(exc),
|
|
)
|
|
|
|
def _apply_device_payload(self, payload: dict[str, Any], *, rebuild_only: bool = False) -> None:
|
|
if "input_mode" in payload or "mode" in payload:
|
|
self._set_input_mode(str(payload.get("input_mode") or payload.get("mode")))
|
|
|
|
indexes = payload.get("camera_indexes")
|
|
urls = payload.get("camera_urls")
|
|
width = payload.get("width")
|
|
height = payload.get("height")
|
|
fps = payload.get("fps")
|
|
should_rebuild = any(value is not None for value in (indexes, urls, width, height, fps))
|
|
if should_rebuild:
|
|
self.rebuild_cameras(
|
|
camera_indexes=self._coerce_indexes(indexes) if indexes is not None else None,
|
|
camera_urls=self._coerce_urls(urls) if urls is not None else None,
|
|
width=int(width) if width else None,
|
|
height=int(height) if height else None,
|
|
fps=int(fps) if fps else None,
|
|
)
|
|
elif rebuild_only:
|
|
self.rebuild_cameras()
|
|
|
|
def _set_input_mode(self, mode: str) -> None:
|
|
normalized = "dual_redundant" if mode == "dual" else mode
|
|
if normalized == "auto":
|
|
normalized = "dual_redundant" if len(self.cameras) >= 2 else "single"
|
|
if normalized not in {"single", "dual_redundant", "single_fallback", "calibrated_3d"}:
|
|
raise ValueError(f"Unsupported input mode: {mode}")
|
|
self.state.mode = normalized
|
|
|
|
def _coerce_indexes(self, value: Any) -> tuple[int, ...]:
|
|
if value is None:
|
|
return self.config.camera_indexes
|
|
if isinstance(value, str):
|
|
return tuple(int(item.strip()) for item in value.split(",") if item.strip())
|
|
if isinstance(value, Iterable):
|
|
return tuple(int(item) for item in value)
|
|
raise ValueError("camera_indexes must be a list or comma-separated string.")
|
|
|
|
def _coerce_urls(self, value: Any) -> tuple[str, ...]:
|
|
if value is None:
|
|
return self.config.camera_urls
|
|
if isinstance(value, str):
|
|
return tuple(item.strip() for item in value.split(",") if item.strip())
|
|
if isinstance(value, Iterable):
|
|
return tuple(str(item).strip() for item in value if str(item).strip())
|
|
raise ValueError("camera_urls must be a list or comma-separated string.")
|
|
|
|
async def handler(self, websocket: Any, path: str | None = None) -> None:
|
|
if path is not None and path != self.config.path:
|
|
await websocket.close(code=1008, reason="Unsupported motion agent path")
|
|
return
|
|
self.clients.add(websocket)
|
|
await websocket.send(self.status_event().to_json())
|
|
try:
|
|
async for message in websocket:
|
|
command, _request_id, _payload = self._parse_command(message)
|
|
if command:
|
|
await websocket.send((await self.handle_command(message)).to_json())
|
|
else:
|
|
await websocket.send(self.status_event().to_json())
|
|
finally:
|
|
self.clients.discard(websocket)
|
|
|
|
async def heartbeat_loop(self) -> None:
|
|
interval = max(0.1, self.config.heartbeat_interval_ms / 1000)
|
|
while not self._stop.is_set():
|
|
await self.broadcast(HeartbeatEvent())
|
|
await asyncio.sleep(interval)
|
|
|
|
async def start_recognition_subprocess(self) -> None:
|
|
if self._recognition_subprocess and self._recognition_subprocess.returncode is None:
|
|
return
|
|
command = [
|
|
sys.executable,
|
|
"-m",
|
|
"motion_agent.worker",
|
|
"--camera-indexes",
|
|
",".join(str(index) for index in self.config.camera_indexes),
|
|
"--mode",
|
|
self.config.mode,
|
|
]
|
|
if self.config.camera_urls:
|
|
command.extend(["--camera-urls", ",".join(self.config.camera_urls)])
|
|
if self.config.dry_run:
|
|
command.append("--dry-run")
|
|
self._recognition_subprocess = await asyncio.create_subprocess_exec(
|
|
*command,
|
|
"--width",
|
|
str(self.config.camera_width),
|
|
"--height",
|
|
str(self.config.camera_height),
|
|
"--fps",
|
|
str(self.config.camera_fps),
|
|
"--max-event-hz",
|
|
str(self.config.max_event_hz),
|
|
"--max-skeleton-hz",
|
|
str(self.config.max_skeleton_hz if self.debug_skeleton_enabled else 0),
|
|
stdout=asyncio.subprocess.PIPE,
|
|
)
|
|
|
|
async def restart_recognition_subprocess(self) -> None:
|
|
await self.stop_recognition_subprocess()
|
|
await self.start_recognition_subprocess()
|
|
|
|
async def stop_recognition_subprocess(self) -> None:
|
|
process = self._recognition_subprocess
|
|
self._recognition_subprocess = None
|
|
if process is None or process.returncode is not None:
|
|
return
|
|
process.terminate()
|
|
try:
|
|
await asyncio.wait_for(process.wait(), timeout=2)
|
|
except asyncio.TimeoutError:
|
|
process.kill()
|
|
await process.wait()
|
|
|
|
async def recognition_subprocess_loop(self) -> None:
|
|
while not self._stop.is_set():
|
|
process = self._recognition_subprocess
|
|
if process is None or process.stdout is None:
|
|
await asyncio.sleep(0.1)
|
|
continue
|
|
line = await process.stdout.readline()
|
|
if not line:
|
|
if self._recognition_subprocess is process and process.returncode is not None:
|
|
self.devices_open = False
|
|
if self.last_error is None and process.returncode not in {0, None}:
|
|
self.last_error = f"Motion recognition worker exited with code {process.returncode}."
|
|
await self.broadcast(self.status_event(connected=True))
|
|
await asyncio.sleep(0.1)
|
|
continue
|
|
try:
|
|
message = json.loads(line.decode("utf-8"))
|
|
except json.JSONDecodeError:
|
|
continue
|
|
kind = message.get("kind")
|
|
payload = message.get("payload")
|
|
if kind == "status" and isinstance(payload, dict):
|
|
self.devices_open = bool(payload.get("devices_open", self.devices_open))
|
|
if payload.get("recognizer"):
|
|
self.recognizer_name = str(payload["recognizer"])
|
|
self.fps = float(payload.get("fps", self.fps) or 0)
|
|
self.recognition_fps = float(payload.get("recognition_fps", self.recognition_fps) or 0)
|
|
self.last_error = payload.get("error")
|
|
await self.broadcast(self.status_event(connected=True))
|
|
elif kind == "skeleton" and isinstance(payload, dict):
|
|
if self.debug_skeleton_enabled:
|
|
await self.broadcast_payload(payload)
|
|
elif kind == "observations" and isinstance(payload, list):
|
|
observations = [
|
|
GestureObservation(
|
|
gesture=item.get("gesture"),
|
|
confidence=float(item.get("confidence", 0)),
|
|
intensity=float(item.get("intensity", 1)),
|
|
timestamp_ms=item.get("timestamp_ms"),
|
|
camera_id=str(item.get("camera_id", "unknown")),
|
|
)
|
|
for item in payload
|
|
if isinstance(item, dict) and item.get("gesture")
|
|
]
|
|
if observations and self.armed and not self.paused:
|
|
event = self._accept_observations(observations)
|
|
if event is not None:
|
|
self.last_gesture = event.gesture
|
|
await self.broadcast(event)
|
|
|
|
def _accept_observations(self, observations: list[GestureObservation]) -> GestureEvent | None:
|
|
if not observations:
|
|
return None
|
|
observations = [item for item in observations if item.gesture in self.enabled_gestures]
|
|
if not observations:
|
|
return None
|
|
selected = observations[0]
|
|
fusion: dict[str, Any] | None = None
|
|
if self.state.mode in {"dual_redundant", "calibrated_3d"} and len(observations) > 1:
|
|
selected, fusion = self._fuse_observations(observations)
|
|
event = self.state.accept(selected)
|
|
if event is not None and fusion is not None:
|
|
event = replace(
|
|
event,
|
|
camera_id="fusion",
|
|
fusion=fusion,
|
|
payload={**event.payload, "fusion": fusion},
|
|
)
|
|
return event
|
|
|
|
def _fuse_observations(
|
|
self,
|
|
observations: list[GestureObservation],
|
|
) -> tuple[GestureObservation, dict[str, Any] | None]:
|
|
ordered = sorted(observations, key=lambda item: item.confidence, reverse=True)
|
|
best = ordered[0]
|
|
same = [item for item in ordered if item.gesture == best.gesture]
|
|
if len(same) >= 2:
|
|
confidence = min(1.0, sum(item.confidence for item in same) / len(same) + 0.06)
|
|
intensity = sum(item.intensity for item in same) / len(same)
|
|
self.last_fusion_reason = "matched_observations"
|
|
return (
|
|
GestureObservation(
|
|
gesture=best.gesture,
|
|
confidence=confidence,
|
|
intensity=intensity,
|
|
timestamp_ms=best.timestamp_ms,
|
|
camera_id="fusion",
|
|
),
|
|
{
|
|
"source_cameras": [item.camera_id for item in same],
|
|
"window_ms": self.fusion_window_ms,
|
|
"reason": self.last_fusion_reason,
|
|
},
|
|
)
|
|
if len(ordered) > 1 and best.confidence - ordered[1].confidence < self.fusion_conflict_delta:
|
|
self.last_fusion_reason = "conflict_ignored"
|
|
return (
|
|
GestureObservation(
|
|
gesture=best.gesture,
|
|
confidence=0,
|
|
intensity=best.intensity,
|
|
timestamp_ms=best.timestamp_ms,
|
|
camera_id=best.camera_id,
|
|
),
|
|
{
|
|
"source_cameras": [item.camera_id for item in ordered],
|
|
"window_ms": self.fusion_window_ms,
|
|
"reason": self.last_fusion_reason,
|
|
},
|
|
)
|
|
self.last_fusion_reason = "highest_confidence"
|
|
return (
|
|
best,
|
|
{
|
|
"source_cameras": [item.camera_id for item in ordered],
|
|
"window_ms": self.fusion_window_ms,
|
|
"reason": self.last_fusion_reason,
|
|
},
|
|
)
|
|
|
|
async def run(self) -> None:
|
|
try:
|
|
import websockets
|
|
except ImportError as exc:
|
|
raise RuntimeError("websockets is required to run the motion agent server.") from exc
|
|
|
|
async with websockets.serve(self.handler, self.config.host, self.config.port):
|
|
print(f"Motion agent listening on {self.config.websocket_url}", flush=True)
|
|
await self.start_recognition_subprocess()
|
|
heartbeat_task = asyncio.create_task(self.heartbeat_loop())
|
|
recognition_task = asyncio.create_task(self.recognition_subprocess_loop())
|
|
try:
|
|
await self._stop.wait()
|
|
finally:
|
|
await self.stop_recognition_subprocess()
|
|
heartbeat_task.cancel()
|
|
recognition_task.cancel()
|
|
with suppress(asyncio.CancelledError):
|
|
await heartbeat_task
|
|
with suppress(asyncio.CancelledError):
|
|
await recognition_task
|
|
|
|
def stop(self) -> None:
|
|
self._stop.set()
|