release: bump version to 0.71.0
This commit is contained in:
@@ -70,13 +70,14 @@ class UsbCameraInput:
|
||||
"or start the agent with --dry-run for protocol testing."
|
||||
) from exc
|
||||
|
||||
capture = cv2.VideoCapture(self.spec.index)
|
||||
capture = cv2.VideoCapture(self.spec.index, cv2.CAP_V4L2)
|
||||
if not capture or not capture.isOpened():
|
||||
raise MotionAgentCameraError(f"Unable to open USB camera index {self.spec.index}.")
|
||||
|
||||
capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.spec.width)
|
||||
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.spec.height)
|
||||
capture.set(cv2.CAP_PROP_FPS, self.spec.fps)
|
||||
capture.set(cv2.CAP_PROP_BUFFERSIZE, 1)
|
||||
self._capture = capture
|
||||
|
||||
def read(self) -> Any:
|
||||
@@ -116,6 +117,7 @@ class UrlCameraInput:
|
||||
f"Unable to open camera URL {self.spec.url}. Check the stream URL, "
|
||||
"firewall, and LAN reachability."
|
||||
)
|
||||
capture.set(cv2.CAP_PROP_BUFFERSIZE, 1)
|
||||
self._capture = capture
|
||||
|
||||
def read(self) -> Any:
|
||||
|
||||
@@ -18,7 +18,11 @@ def build_parser() -> argparse.ArgumentParser:
|
||||
parser.add_argument("--port", type=int, default=None, help="WebSocket bind port.")
|
||||
parser.add_argument("--camera-indexes", default=None, help="Comma-separated USB camera indexes.")
|
||||
parser.add_argument("--camera-urls", default=None, help="Comma-separated RTSP/HTTP camera URLs.")
|
||||
parser.add_argument("--mode", choices=["auto", "single", "dual"], default=None)
|
||||
parser.add_argument(
|
||||
"--mode",
|
||||
choices=["auto", "single", "dual", "dual_redundant", "single_fallback", "calibrated_3d"],
|
||||
default=None,
|
||||
)
|
||||
parser.add_argument("--dry-run", action="store_true", help="Start without camera/CV dependencies.")
|
||||
return parser
|
||||
|
||||
|
||||
@@ -5,6 +5,8 @@ from __future__ import annotations
|
||||
import os
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
SUPPORTED_INPUT_MODES = {"auto", "single", "dual", "dual_redundant", "single_fallback", "calibrated_3d"}
|
||||
|
||||
|
||||
def _parse_camera_indexes(raw: str | None) -> tuple[int, ...]:
|
||||
if not raw:
|
||||
@@ -31,14 +33,17 @@ class MotionAgentConfig:
|
||||
path: str = "/ws/gestures"
|
||||
camera_indexes: tuple[int, ...] = field(default_factory=lambda: (0,))
|
||||
camera_urls: tuple[str, ...] = field(default_factory=tuple)
|
||||
camera_width: int = 1280
|
||||
camera_height: int = 720
|
||||
camera_width: int = 640
|
||||
camera_height: int = 360
|
||||
camera_fps: int = 30
|
||||
mode: str = "auto"
|
||||
confidence_threshold: float = 0.72
|
||||
cooldown_ms: int = 450
|
||||
heartbeat_interval_ms: int = 1000
|
||||
max_event_hz: int = 20
|
||||
max_event_hz: int = 15
|
||||
max_skeleton_hz: int = 8
|
||||
fusion_window_ms: int = 120
|
||||
fusion_conflict_delta: float = 0.18
|
||||
dry_run: bool = False
|
||||
|
||||
@property
|
||||
@@ -53,13 +58,16 @@ class MotionAgentConfig:
|
||||
path=os.getenv("MOTION_AGENT_PATH", "/ws/gestures"),
|
||||
camera_indexes=_parse_camera_indexes(os.getenv("MOTION_AGENT_CAMERA_INDEXES")),
|
||||
camera_urls=_parse_camera_urls(os.getenv("MOTION_AGENT_CAMERA_URLS")),
|
||||
camera_width=int(os.getenv("MOTION_AGENT_CAMERA_WIDTH", "1280")),
|
||||
camera_height=int(os.getenv("MOTION_AGENT_CAMERA_HEIGHT", "720")),
|
||||
camera_width=int(os.getenv("MOTION_AGENT_CAMERA_WIDTH", "640")),
|
||||
camera_height=int(os.getenv("MOTION_AGENT_CAMERA_HEIGHT", "360")),
|
||||
camera_fps=int(os.getenv("MOTION_AGENT_CAMERA_FPS", "30")),
|
||||
mode=os.getenv("MOTION_AGENT_MODE", "auto"),
|
||||
confidence_threshold=float(os.getenv("MOTION_AGENT_CONFIDENCE_THRESHOLD", "0.72")),
|
||||
cooldown_ms=int(os.getenv("MOTION_AGENT_COOLDOWN_MS", "450")),
|
||||
heartbeat_interval_ms=int(os.getenv("MOTION_AGENT_HEARTBEAT_MS", "1000")),
|
||||
max_event_hz=int(os.getenv("MOTION_AGENT_MAX_EVENT_HZ", "20")),
|
||||
max_event_hz=int(os.getenv("MOTION_AGENT_MAX_EVENT_HZ", "15")),
|
||||
max_skeleton_hz=int(os.getenv("MOTION_AGENT_MAX_SKELETON_HZ", "8")),
|
||||
fusion_window_ms=int(os.getenv("MOTION_AGENT_FUSION_WINDOW_MS", "120")),
|
||||
fusion_conflict_delta=float(os.getenv("MOTION_AGENT_FUSION_CONFLICT_DELTA", "0.18")),
|
||||
dry_run=os.getenv("MOTION_AGENT_DRY_RUN", "").lower() in {"1", "true", "yes"},
|
||||
)
|
||||
|
||||
@@ -7,8 +7,23 @@ import time
|
||||
from dataclasses import asdict, dataclass, field
|
||||
from typing import Any, Literal
|
||||
|
||||
GestureName = Literal["rotate_left", "rotate_right", "zoom_in", "zoom_out", "confirm"]
|
||||
PROTOCOL_VERSION = "motion.v2"
|
||||
|
||||
GestureName = Literal[
|
||||
"rotate_left",
|
||||
"rotate_right",
|
||||
"rotate_up",
|
||||
"rotate_down",
|
||||
"zoom_in",
|
||||
"zoom_out",
|
||||
"focus_prev",
|
||||
"focus_next",
|
||||
"layer_prev",
|
||||
"layer_next",
|
||||
"confirm",
|
||||
]
|
||||
GesturePhase = Literal["start", "active", "end", "discrete"]
|
||||
InputMode = Literal["single", "dual_redundant", "single_fallback", "calibrated_3d"]
|
||||
|
||||
|
||||
def now_ms() -> int:
|
||||
@@ -25,6 +40,10 @@ class GestureEvent:
|
||||
seq: int = 0
|
||||
source: str = "motion-agent"
|
||||
mode: str = "single"
|
||||
protocol_version: str = PROTOCOL_VERSION
|
||||
camera_id: str = "unknown"
|
||||
input_mode: str = "single"
|
||||
fusion: dict[str, Any] | None = None
|
||||
payload: dict[str, Any] = field(default_factory=dict)
|
||||
type: Literal["gesture"] = "gesture"
|
||||
|
||||
@@ -41,9 +60,17 @@ class StatusEvent:
|
||||
camera_count: int
|
||||
active_camera_ids: tuple[str, ...] = ()
|
||||
mode: str = "single"
|
||||
protocol_version: str = PROTOCOL_VERSION
|
||||
input_mode: str = "single"
|
||||
armed: bool = False
|
||||
paused: bool = False
|
||||
devices_open: bool = False
|
||||
recognizer: str = "mediapipe-opencv"
|
||||
fps: float = 0.0
|
||||
recognition_fps: float = 0.0
|
||||
last_gesture: str | None = None
|
||||
last_fusion_reason: str | None = None
|
||||
enabled_gestures: tuple[str, ...] = ()
|
||||
error: str | None = None
|
||||
timestamp_ms: int = field(default_factory=now_ms)
|
||||
source: str = "motion-agent"
|
||||
@@ -59,6 +86,7 @@ class StatusEvent:
|
||||
@dataclass(frozen=True)
|
||||
class HeartbeatEvent:
|
||||
timestamp_ms: int = field(default_factory=now_ms)
|
||||
protocol_version: str = PROTOCOL_VERSION
|
||||
source: str = "motion-agent"
|
||||
type: Literal["heartbeat"] = "heartbeat"
|
||||
|
||||
@@ -87,6 +115,8 @@ class SkeletonEvent:
|
||||
timestamp_ms: int = field(default_factory=now_ms)
|
||||
source: str = "motion-agent"
|
||||
mode: str = "single"
|
||||
protocol_version: str = PROTOCOL_VERSION
|
||||
input_mode: str = "single"
|
||||
type: Literal["skeleton"] = "skeleton"
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
@@ -94,3 +124,22 @@ class SkeletonEvent:
|
||||
|
||||
def to_json(self) -> str:
|
||||
return json.dumps(self.to_dict(), ensure_ascii=False, separators=(",", ":"))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CommandResultEvent:
|
||||
command: str
|
||||
request_id: str | None = None
|
||||
ok: bool = True
|
||||
status: dict[str, Any] = field(default_factory=dict)
|
||||
error: str | None = None
|
||||
timestamp_ms: int = field(default_factory=now_ms)
|
||||
protocol_version: str = PROTOCOL_VERSION
|
||||
source: str = "motion-agent"
|
||||
type: Literal["command_result"] = "command_result"
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return asdict(self)
|
||||
|
||||
def to_json(self) -> str:
|
||||
return json.dumps(self.to_dict(), ensure_ascii=False, separators=(",", ":"))
|
||||
|
||||
@@ -7,12 +7,64 @@ from any specific model implementation.
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import urllib.request
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Protocol
|
||||
|
||||
from .cameras import MotionAgentDependencyError
|
||||
from .events import GestureName, SkeletonEvent, SkeletonJoint, now_ms
|
||||
|
||||
POSE_MODEL_URL = (
|
||||
"https://storage.googleapis.com/mediapipe-models/pose_landmarker/"
|
||||
"pose_landmarker_lite/float16/latest/pose_landmarker_lite.task"
|
||||
)
|
||||
POSE_MODEL_CACHE_PATH = Path.home() / ".cache" / "planet" / "motion_agent" / "pose_landmarker_lite.task"
|
||||
|
||||
POSE_JOINTS = [
|
||||
(0, "nose"),
|
||||
(7, "left_ear"),
|
||||
(8, "right_ear"),
|
||||
(11, "left_shoulder"),
|
||||
(12, "right_shoulder"),
|
||||
(13, "left_elbow"),
|
||||
(14, "right_elbow"),
|
||||
(15, "left_wrist"),
|
||||
(16, "right_wrist"),
|
||||
]
|
||||
POSE_BONES = [
|
||||
("left_shoulder", "left_elbow"),
|
||||
("left_elbow", "left_wrist"),
|
||||
("right_shoulder", "right_elbow"),
|
||||
("right_elbow", "right_wrist"),
|
||||
("left_shoulder", "right_shoulder"),
|
||||
]
|
||||
|
||||
LEFT_WRIST_LAYER_DELTA_Y = 0.05
|
||||
HEAD_TILT_DELTA_Y = 0.035
|
||||
ARM_PATTERN_TERMINAL_TOLERANCE_DEG = 32
|
||||
ARM_PATTERN_UPPER_TOLERANCE_DEG = 34
|
||||
ARM_PATTERN_MIN_SEGMENT = 0.045
|
||||
ARM_PATTERN_MIN_SIDE_REACH = 0.06
|
||||
ARM_PATTERN_MIN_VERTICAL_REACH = 0.055
|
||||
MIN_GESTURE_INTENSITY = 0.45
|
||||
ARM_PATTERN_INTENSITY_SCALE = 5
|
||||
WRIST_LAYER_INTENSITY_SCALE = 9
|
||||
HEAD_TILT_INTENSITY_SCALE = 12
|
||||
ZOOM_CLOSE_WRIST_SPREAD_FACTOR = 1.28
|
||||
ZOOM_SUPPRESS_WRIST_SPREAD_FACTOR = 1.18
|
||||
ZOOM_TREND_MIN_WRIST_DELTA = 0.010
|
||||
ZOOM_TREND_HEIGHT_TOLERANCE = 0.18
|
||||
ZOOM_TREND_INTENSITY_SCALE = 14
|
||||
ZOOM_TREND_MIN_INTENSITY = 0.6
|
||||
LEFT_ARM_REST_HANGING_BELOW_SHOULDER = 0.13
|
||||
ZOOM_HOLD_HEIGHT_TOLERANCE = 0.18
|
||||
ZOOM_HOLD_WRIST_BELOW_SHOULDER_LIMIT = 0.10
|
||||
ZOOM_HOLD_SPREAD_FACTOR = 1.30
|
||||
ZOOM_HOLD_CLOSE_FACTOR = 0.85
|
||||
ZOOM_HOLD_ELBOW_OUT_FACTOR = 0.25
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GestureObservation:
|
||||
@@ -20,6 +72,7 @@ class GestureObservation:
|
||||
confidence: float
|
||||
intensity: float = 1.0
|
||||
timestamp_ms: int | None = None
|
||||
camera_id: str = "unknown"
|
||||
|
||||
|
||||
class GestureRecognizer(Protocol):
|
||||
@@ -46,16 +99,47 @@ class MediaPipeGestureRecognizer:
|
||||
def __init__(self) -> None:
|
||||
try:
|
||||
import cv2 # noqa: F401
|
||||
import mediapipe # noqa: F401
|
||||
import mediapipe as mp
|
||||
from mediapipe.tasks.python.core import base_options as base_options_module
|
||||
from mediapipe.tasks.python.vision import pose_landmarker
|
||||
from mediapipe.tasks.python.vision.core import vision_task_running_mode
|
||||
except ImportError as exc:
|
||||
raise MotionAgentDependencyError(
|
||||
"MediaPipe and OpenCV are required for live gesture recognition. "
|
||||
"Add mediapipe and opencv-python with uv, or use --dry-run for protocol testing."
|
||||
) from exc
|
||||
model_path = _resolve_pose_model_path()
|
||||
options = pose_landmarker.PoseLandmarkerOptions(
|
||||
base_options=base_options_module.BaseOptions(model_asset_path=str(model_path)),
|
||||
running_mode=vision_task_running_mode.VisionTaskRunningMode.VIDEO,
|
||||
num_poses=1,
|
||||
)
|
||||
self._mp = mp
|
||||
self._cv2 = cv2
|
||||
self._landmarker = pose_landmarker.PoseLandmarker.create_from_options(options)
|
||||
self._previous_joints: list[SkeletonJoint] = []
|
||||
self._latest_joints: list[SkeletonJoint] = []
|
||||
self._latest_timestamp_ms = 0
|
||||
self._state: dict[str, str | None] = {"active_pattern_gesture": None}
|
||||
|
||||
def recognize(self, frame: Any) -> GestureObservation | None:
|
||||
_ = frame
|
||||
return None
|
||||
joints = self._detect_joints(frame)
|
||||
self._latest_joints = joints
|
||||
self._latest_timestamp_ms = now_ms()
|
||||
if not joints:
|
||||
self._previous_joints = []
|
||||
self._state["active_pattern_gesture"] = None
|
||||
return None
|
||||
observation = _recognize_gesture(joints, self._previous_joints, self._state)
|
||||
self._previous_joints = joints
|
||||
if observation is None:
|
||||
return None
|
||||
return GestureObservation(
|
||||
gesture=observation["gesture"],
|
||||
confidence=observation["confidence"],
|
||||
intensity=observation["intensity"],
|
||||
timestamp_ms=self._latest_timestamp_ms,
|
||||
)
|
||||
|
||||
def debug_skeleton(
|
||||
self,
|
||||
@@ -66,8 +150,35 @@ class MediaPipeGestureRecognizer:
|
||||
matched_gesture: GestureName | None = None,
|
||||
confidence: float = 0.0,
|
||||
) -> SkeletonEvent | None:
|
||||
_ = frame, camera_id, mode, matched_gesture, confidence
|
||||
return None
|
||||
_ = frame
|
||||
if not self._latest_joints:
|
||||
return None
|
||||
return SkeletonEvent(
|
||||
joints=self._latest_joints,
|
||||
bones=POSE_BONES,
|
||||
matched_gesture=matched_gesture,
|
||||
confidence=confidence,
|
||||
camera_id=camera_id,
|
||||
mode=mode,
|
||||
)
|
||||
|
||||
def _detect_joints(self, frame: Any) -> list[SkeletonJoint]:
|
||||
rgb = self._cv2.cvtColor(frame, self._cv2.COLOR_BGR2RGB)
|
||||
image = self._mp.Image(image_format=self._mp.ImageFormat.SRGB, data=rgb)
|
||||
result = self._landmarker.detect_for_video(image, now_ms())
|
||||
if not result.pose_landmarks:
|
||||
return []
|
||||
landmarks = result.pose_landmarks[0]
|
||||
joints: list[SkeletonJoint] = []
|
||||
for index, joint_id in POSE_JOINTS:
|
||||
if index >= len(landmarks):
|
||||
continue
|
||||
point = landmarks[index]
|
||||
x = _clamp01(float(point.x))
|
||||
y = _clamp01(float(point.y))
|
||||
confidence = _clamp01(float(getattr(point, "visibility", getattr(point, "presence", 1.0))))
|
||||
joints.append(SkeletonJoint(joint_id, x, y, confidence))
|
||||
return joints
|
||||
|
||||
|
||||
class NullGestureRecognizer:
|
||||
@@ -116,3 +227,275 @@ class NullGestureRecognizer:
|
||||
camera_id=camera_id,
|
||||
mode=mode,
|
||||
)
|
||||
|
||||
|
||||
def _resolve_pose_model_path() -> Path:
|
||||
configured = os.getenv("MOTION_AGENT_POSE_MODEL_PATH")
|
||||
path = Path(configured).expanduser() if configured else POSE_MODEL_CACHE_PATH
|
||||
if path.exists():
|
||||
return path
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
try:
|
||||
urllib.request.urlretrieve(POSE_MODEL_URL, path)
|
||||
except Exception as exc:
|
||||
raise MotionAgentDependencyError(
|
||||
"MediaPipe pose model is missing and could not be downloaded. "
|
||||
f"Set MOTION_AGENT_POSE_MODEL_PATH to a local .task file or download {POSE_MODEL_URL}."
|
||||
) from exc
|
||||
return path
|
||||
|
||||
|
||||
def _clamp01(value: float) -> float:
|
||||
return max(0.0, min(1.0, value))
|
||||
|
||||
|
||||
def _get_joint(joints: list[SkeletonJoint], joint_id: str) -> SkeletonJoint | None:
|
||||
return next((joint for joint in joints if joint.id == joint_id), None)
|
||||
|
||||
|
||||
def _vector_between(start: SkeletonJoint | None, end: SkeletonJoint | None) -> dict[str, float] | None:
|
||||
if start is None or end is None:
|
||||
return None
|
||||
dx = end.x - start.x
|
||||
dy = end.y - start.y
|
||||
return {"dx": dx, "dy": dy, "length": (dx * dx + dy * dy) ** 0.5}
|
||||
|
||||
|
||||
def _vector_angle_deg(vector: dict[str, float]) -> float:
|
||||
import math
|
||||
|
||||
return math.atan2(vector["dy"], vector["dx"]) * 180 / math.pi
|
||||
|
||||
|
||||
def _normalize_angle_delta(angle: float, target: float) -> float:
|
||||
delta = angle - target
|
||||
while delta > 180:
|
||||
delta -= 360
|
||||
while delta < -180:
|
||||
delta += 360
|
||||
return abs(delta)
|
||||
|
||||
|
||||
def _is_angle_near(angle: float, target: float, tolerance_deg: float) -> bool:
|
||||
return _normalize_angle_delta(angle, target) <= tolerance_deg
|
||||
|
||||
|
||||
def _is_horizontal_arm(upper_vector: dict[str, float] | None) -> bool:
|
||||
if not upper_vector or upper_vector["length"] < ARM_PATTERN_MIN_SEGMENT:
|
||||
return False
|
||||
angle = _vector_angle_deg(upper_vector)
|
||||
return _is_angle_near(angle, 0, ARM_PATTERN_UPPER_TOLERANCE_DEG) or _is_angle_near(
|
||||
angle, 180, ARM_PATTERN_UPPER_TOLERANCE_DEG
|
||||
)
|
||||
|
||||
|
||||
def _is_terminal_toward(vector: dict[str, float] | None, target_angle: float) -> bool:
|
||||
if not vector or vector["length"] < ARM_PATTERN_MIN_SEGMENT:
|
||||
return False
|
||||
return _is_angle_near(_vector_angle_deg(vector), target_angle, ARM_PATTERN_TERMINAL_TOLERANCE_DEG)
|
||||
|
||||
|
||||
def _gesture(gesture: GestureName, confidence: float, intensity: float) -> dict[str, Any]:
|
||||
return {"gesture": gesture, "confidence": confidence, "intensity": intensity}
|
||||
|
||||
|
||||
def _get_right_arm_pattern(
|
||||
right_shoulder: SkeletonJoint,
|
||||
right_elbow: SkeletonJoint,
|
||||
right_wrist: SkeletonJoint,
|
||||
) -> dict[str, Any] | None:
|
||||
upper = _vector_between(right_shoulder, right_elbow)
|
||||
terminal = _vector_between(right_elbow, right_wrist)
|
||||
if not upper or not terminal:
|
||||
return None
|
||||
intensity = min(1.0, max(MIN_GESTURE_INTENSITY, terminal["length"] * ARM_PATTERN_INTENSITY_SCALE))
|
||||
if _is_terminal_toward(terminal, 180) and right_wrist.x < right_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH:
|
||||
return _gesture("rotate_right", 0.82, intensity)
|
||||
if _is_terminal_toward(terminal, 0) and right_wrist.x > right_shoulder.x + ARM_PATTERN_MIN_SIDE_REACH:
|
||||
return _gesture("rotate_left", 0.82, intensity)
|
||||
if (
|
||||
_is_horizontal_arm(upper)
|
||||
and _is_terminal_toward(terminal, -90)
|
||||
and right_wrist.y < right_elbow.y - ARM_PATTERN_MIN_VERTICAL_REACH
|
||||
):
|
||||
return _gesture("rotate_up", 0.8, intensity)
|
||||
if (
|
||||
_is_horizontal_arm(upper)
|
||||
and _is_terminal_toward(terminal, 90)
|
||||
and right_wrist.y > right_elbow.y + ARM_PATTERN_MIN_VERTICAL_REACH
|
||||
):
|
||||
return _gesture("rotate_down", 0.8, intensity)
|
||||
return None
|
||||
|
||||
|
||||
def _is_zoom_candidate_pose(
|
||||
left_shoulder: SkeletonJoint,
|
||||
left_elbow: SkeletonJoint,
|
||||
left_wrist: SkeletonJoint,
|
||||
right_shoulder: SkeletonJoint,
|
||||
right_elbow: SkeletonJoint,
|
||||
right_wrist: SkeletonJoint,
|
||||
shoulder_width: float,
|
||||
) -> bool:
|
||||
wrists_apart = abs(right_wrist.x - left_wrist.x)
|
||||
both_hands_outside = (
|
||||
left_wrist.x < left_elbow.x - ARM_PATTERN_MIN_SIDE_REACH * 0.25
|
||||
and left_wrist.x < left_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH * 0.55
|
||||
and right_wrist.x > right_elbow.x + ARM_PATTERN_MIN_SIDE_REACH * 0.25
|
||||
and right_wrist.x > right_shoulder.x + ARM_PATTERN_MIN_SIDE_REACH * 0.55
|
||||
)
|
||||
both_elbows_participating = (
|
||||
left_elbow.x <= left_shoulder.x + ARM_PATTERN_MIN_SIDE_REACH
|
||||
and right_elbow.x >= right_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH
|
||||
)
|
||||
hands_near_center = (
|
||||
left_elbow.x < left_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH * 0.5
|
||||
and right_elbow.x > right_shoulder.x + ARM_PATTERN_MIN_SIDE_REACH * 0.5
|
||||
and left_wrist.x > left_elbow.x
|
||||
and right_wrist.x < right_elbow.x
|
||||
and wrists_apart < shoulder_width * ZOOM_CLOSE_WRIST_SPREAD_FACTOR
|
||||
)
|
||||
return (
|
||||
both_hands_outside
|
||||
and both_elbows_participating
|
||||
and wrists_apart > shoulder_width * ZOOM_SUPPRESS_WRIST_SPREAD_FACTOR
|
||||
) or hands_near_center
|
||||
|
||||
|
||||
def _is_left_arm_at_rest(left_shoulder: SkeletonJoint, left_elbow: SkeletonJoint, left_wrist: SkeletonJoint) -> bool:
|
||||
return (
|
||||
left_wrist.y >= left_shoulder.y + LEFT_ARM_REST_HANGING_BELOW_SHOULDER
|
||||
and left_wrist.x >= left_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH
|
||||
and left_elbow.x >= left_shoulder.x - ARM_PATTERN_MIN_SIDE_REACH
|
||||
)
|
||||
|
||||
|
||||
def _get_zoom_trend(
|
||||
left_wrist: SkeletonJoint,
|
||||
right_wrist: SkeletonJoint,
|
||||
previous_left_wrist: SkeletonJoint | None,
|
||||
previous_right_wrist: SkeletonJoint | None,
|
||||
) -> dict[str, Any] | None:
|
||||
if previous_left_wrist is None or previous_right_wrist is None:
|
||||
return None
|
||||
if abs(right_wrist.y - left_wrist.y) > ZOOM_TREND_HEIGHT_TOLERANCE:
|
||||
return None
|
||||
left_moved = abs(left_wrist.x - previous_left_wrist.x)
|
||||
right_moved = abs(right_wrist.x - previous_right_wrist.x)
|
||||
if left_moved < ZOOM_TREND_MIN_WRIST_DELTA or right_moved < ZOOM_TREND_MIN_WRIST_DELTA:
|
||||
return None
|
||||
spread_delta = abs(right_wrist.x - left_wrist.x) - abs(previous_right_wrist.x - previous_left_wrist.x)
|
||||
min_spread_delta = ZOOM_TREND_MIN_WRIST_DELTA * 2
|
||||
intensity = min(1.0, max(ZOOM_TREND_MIN_INTENSITY, (left_moved + right_moved) * ZOOM_TREND_INTENSITY_SCALE))
|
||||
if spread_delta > min_spread_delta:
|
||||
return _gesture("zoom_in", 0.88, intensity)
|
||||
if spread_delta < -min_spread_delta:
|
||||
return _gesture("zoom_out", 0.86, intensity)
|
||||
return None
|
||||
|
||||
|
||||
def _get_zoom_hold_pose(
|
||||
left_shoulder: SkeletonJoint,
|
||||
left_elbow: SkeletonJoint,
|
||||
left_wrist: SkeletonJoint,
|
||||
right_shoulder: SkeletonJoint,
|
||||
right_elbow: SkeletonJoint,
|
||||
right_wrist: SkeletonJoint,
|
||||
shoulder_width: float,
|
||||
) -> dict[str, Any] | None:
|
||||
if abs(right_wrist.y - left_wrist.y) > ZOOM_HOLD_HEIGHT_TOLERANCE:
|
||||
return None
|
||||
avg_shoulder_y = (left_shoulder.y + right_shoulder.y) / 2
|
||||
wrists_raised = (
|
||||
left_wrist.y <= avg_shoulder_y + ZOOM_HOLD_WRIST_BELOW_SHOULDER_LIMIT
|
||||
and right_wrist.y <= avg_shoulder_y + ZOOM_HOLD_WRIST_BELOW_SHOULDER_LIMIT
|
||||
)
|
||||
if not wrists_raised:
|
||||
return None
|
||||
span = abs(right_wrist.x - left_wrist.x)
|
||||
if span > shoulder_width * ZOOM_HOLD_SPREAD_FACTOR:
|
||||
return _gesture("zoom_in", 0.82, 0.8)
|
||||
elbows_outward = (
|
||||
abs(left_elbow.x - left_shoulder.x) > shoulder_width * ZOOM_HOLD_ELBOW_OUT_FACTOR
|
||||
and abs(right_elbow.x - right_shoulder.x) > shoulder_width * ZOOM_HOLD_ELBOW_OUT_FACTOR
|
||||
)
|
||||
if elbows_outward and span < shoulder_width * ZOOM_HOLD_CLOSE_FACTOR:
|
||||
return _gesture("zoom_out", 0.80, 0.7)
|
||||
return None
|
||||
|
||||
|
||||
def _apply_pose_latch(observation: dict[str, Any] | None, state: dict[str, str | None]) -> dict[str, Any] | None:
|
||||
if observation is None:
|
||||
return None
|
||||
if state.get("active_pattern_gesture") == observation["gesture"]:
|
||||
return None
|
||||
state["active_pattern_gesture"] = observation["gesture"]
|
||||
return observation
|
||||
|
||||
|
||||
def _recognize_gesture(
|
||||
joints: list[SkeletonJoint],
|
||||
previous_joints: list[SkeletonJoint],
|
||||
state: dict[str, str | None],
|
||||
) -> dict[str, Any] | None:
|
||||
left_ear = _get_joint(joints, "left_ear")
|
||||
right_ear = _get_joint(joints, "right_ear")
|
||||
left_wrist = _get_joint(joints, "left_wrist")
|
||||
right_wrist = _get_joint(joints, "right_wrist")
|
||||
left_elbow = _get_joint(joints, "left_elbow")
|
||||
right_elbow = _get_joint(joints, "right_elbow")
|
||||
left_shoulder = _get_joint(joints, "left_shoulder")
|
||||
right_shoulder = _get_joint(joints, "right_shoulder")
|
||||
previous_left_wrist = _get_joint(previous_joints, "left_wrist")
|
||||
previous_right_wrist = _get_joint(previous_joints, "right_wrist")
|
||||
if not all([left_wrist, right_wrist, left_elbow, right_elbow, left_shoulder, right_shoulder]):
|
||||
return None
|
||||
|
||||
assert left_wrist and right_wrist and left_elbow and right_elbow and left_shoulder and right_shoulder
|
||||
trend = _get_zoom_trend(left_wrist, right_wrist, previous_left_wrist, previous_right_wrist)
|
||||
if trend:
|
||||
state["active_pattern_gesture"] = trend["gesture"]
|
||||
return trend
|
||||
|
||||
shoulder_width = max(0.08, abs(right_shoulder.x - left_shoulder.x))
|
||||
left_raised = left_wrist.y < left_shoulder.y - 0.05
|
||||
right_raised = right_wrist.y < right_shoulder.y - 0.05
|
||||
left_delta_y = left_wrist.y - previous_left_wrist.y if previous_left_wrist else 0.0
|
||||
head_tilt_y = right_ear.y - left_ear.y if left_ear and right_ear else 0.0
|
||||
|
||||
if not right_raised and left_raised and left_delta_y < -LEFT_WRIST_LAYER_DELTA_Y:
|
||||
return _gesture("layer_prev", 0.78, min(1.0, abs(left_delta_y) * WRIST_LAYER_INTENSITY_SCALE))
|
||||
if not right_raised and left_raised and left_delta_y > LEFT_WRIST_LAYER_DELTA_Y:
|
||||
return _gesture("layer_next", 0.78, min(1.0, abs(left_delta_y) * WRIST_LAYER_INTENSITY_SCALE))
|
||||
if head_tilt_y < -HEAD_TILT_DELTA_Y:
|
||||
return _gesture("focus_prev", 0.78, min(1.0, abs(head_tilt_y) * HEAD_TILT_INTENSITY_SCALE))
|
||||
if head_tilt_y > HEAD_TILT_DELTA_Y:
|
||||
return _gesture("focus_next", 0.78, min(1.0, abs(head_tilt_y) * HEAD_TILT_INTENSITY_SCALE))
|
||||
|
||||
zoom_pattern = _get_zoom_hold_pose(
|
||||
left_shoulder,
|
||||
left_elbow,
|
||||
left_wrist,
|
||||
right_shoulder,
|
||||
right_elbow,
|
||||
right_wrist,
|
||||
shoulder_width,
|
||||
)
|
||||
if zoom_pattern:
|
||||
state["active_pattern_gesture"] = zoom_pattern["gesture"]
|
||||
return zoom_pattern
|
||||
|
||||
rotate_allowed = not _is_zoom_candidate_pose(
|
||||
left_shoulder,
|
||||
left_elbow,
|
||||
left_wrist,
|
||||
right_shoulder,
|
||||
right_elbow,
|
||||
right_wrist,
|
||||
shoulder_width,
|
||||
) and _is_left_arm_at_rest(left_shoulder, left_elbow, left_wrist)
|
||||
rotate_pattern = _get_right_arm_pattern(right_shoulder, right_elbow, right_wrist) if rotate_allowed else None
|
||||
if rotate_pattern:
|
||||
return _apply_pose_latch(rotate_pattern, state)
|
||||
state["active_pattern_gesture"] = None
|
||||
return None
|
||||
|
||||
@@ -3,12 +3,14 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import sys
|
||||
from collections.abc import Iterable
|
||||
from contextlib import suppress
|
||||
from typing import Any
|
||||
from dataclasses import replace
|
||||
from typing import Any, get_args
|
||||
|
||||
from .cameras import (
|
||||
MotionAgentCameraError,
|
||||
NullCameraInput,
|
||||
UrlCameraInput,
|
||||
UrlCameraSpec,
|
||||
@@ -16,11 +18,21 @@ from .cameras import (
|
||||
UsbCameraSpec,
|
||||
)
|
||||
from .config import MotionAgentConfig
|
||||
from .events import GestureEvent, HeartbeatEvent, SkeletonEvent, StatusEvent
|
||||
from .recognizer import GestureRecognizer, MediaPipeGestureRecognizer, NullGestureRecognizer
|
||||
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,
|
||||
@@ -30,7 +42,10 @@ class MotionAgentServer:
|
||||
) -> None:
|
||||
self.config = config
|
||||
self.cameras = list(cameras) if cameras is not None else self._build_cameras(config)
|
||||
self.recognizer = recognizer if recognizer is not None else self._build_recognizer(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,
|
||||
@@ -39,7 +54,18 @@ class MotionAgentServer:
|
||||
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:
|
||||
@@ -60,33 +86,40 @@ class MotionAgentServer:
|
||||
]
|
||||
return [UsbCameraInput(spec) for spec in specs]
|
||||
|
||||
def _build_recognizer(self, config: MotionAgentConfig) -> GestureRecognizer:
|
||||
if config.dry_run:
|
||||
return NullGestureRecognizer()
|
||||
return MediaPipeGestureRecognizer()
|
||||
|
||||
def _resolve_mode(self) -> str:
|
||||
if self.config.mode in {"single", "dual", "auto"}:
|
||||
if self.config.mode != "auto":
|
||||
return self.config.mode
|
||||
return "dual" if len(self.cameras) >= 2 else "single"
|
||||
|
||||
def open_cameras(self) -> None:
|
||||
opened = []
|
||||
try:
|
||||
for camera in self.cameras:
|
||||
camera.open()
|
||||
opened.append(camera)
|
||||
except Exception:
|
||||
for camera in opened:
|
||||
with suppress(Exception):
|
||||
camera.close()
|
||||
raise
|
||||
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(
|
||||
@@ -94,14 +127,25 @@ class MotionAgentServer:
|
||||
camera_count=len(self.cameras),
|
||||
active_camera_ids=tuple(camera.camera_id for camera in self.cameras),
|
||||
mode=self.state.mode,
|
||||
recognizer=self.recognizer.name,
|
||||
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: GestureEvent | HeartbeatEvent | SkeletonEvent | StatusEvent,
|
||||
event: CommandResultEvent | GestureEvent | HeartbeatEvent | SkeletonEvent | StatusEvent,
|
||||
) -> None:
|
||||
if not self.clients:
|
||||
return
|
||||
@@ -115,6 +159,153 @@ class MotionAgentServer:
|
||||
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")
|
||||
@@ -122,8 +313,12 @@ class MotionAgentServer:
|
||||
self.clients.add(websocket)
|
||||
await websocket.send(self.status_event().to_json())
|
||||
try:
|
||||
async for _message in websocket:
|
||||
await websocket.send(self.status_event().to_json())
|
||||
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)
|
||||
|
||||
@@ -133,38 +328,173 @@ class MotionAgentServer:
|
||||
await self.broadcast(HeartbeatEvent())
|
||||
await asyncio.sleep(interval)
|
||||
|
||||
async def recognition_loop(self) -> None:
|
||||
min_interval = 1 / max(1, self.config.max_event_hz)
|
||||
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:
|
||||
for camera in self.cameras:
|
||||
frame = camera.read()
|
||||
observation = self.recognizer.recognize(frame)
|
||||
matched_gesture = None
|
||||
matched_confidence = 0.0
|
||||
if observation is None:
|
||||
event = None
|
||||
else:
|
||||
event = self.state.accept(observation)
|
||||
if event is not None:
|
||||
self.last_gesture = event.gesture
|
||||
matched_gesture = event.gesture
|
||||
matched_confidence = event.confidence
|
||||
await self.broadcast(event)
|
||||
skeleton = self.recognizer.debug_skeleton(
|
||||
frame,
|
||||
camera_id=camera.camera_id,
|
||||
mode=self.state.mode,
|
||||
matched_gesture=matched_gesture,
|
||||
confidence=matched_confidence,
|
||||
)
|
||||
if skeleton is not None:
|
||||
await self.broadcast(skeleton)
|
||||
except MotionAgentCameraError as exc:
|
||||
self.last_error = str(exc)
|
||||
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))
|
||||
await asyncio.sleep(1)
|
||||
await asyncio.sleep(min_interval)
|
||||
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:
|
||||
@@ -172,21 +502,21 @@ class MotionAgentServer:
|
||||
except ImportError as exc:
|
||||
raise RuntimeError("websockets is required to run the motion agent server.") from exc
|
||||
|
||||
self.open_cameras()
|
||||
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_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
|
||||
self.close_cameras()
|
||||
|
||||
def stop(self) -> None:
|
||||
self._stop.set()
|
||||
|
||||
@@ -41,4 +41,6 @@ class GestureStateMachine:
|
||||
timestamp_ms=timestamp_ms,
|
||||
seq=self._seq,
|
||||
mode=self.mode,
|
||||
input_mode=self.mode,
|
||||
camera_id=observation.camera_id,
|
||||
)
|
||||
|
||||
293
motion_agent/worker.py
Normal file
293
motion_agent/worker.py
Normal file
@@ -0,0 +1,293 @@
|
||||
"""Recognition worker process for Motion Agent.
|
||||
|
||||
The WebSocket server stays in the parent process. This worker owns OpenCV and
|
||||
MediaPipe so native camera/model work cannot block or crash the control plane.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import replace
|
||||
from typing import Any
|
||||
|
||||
from .cameras import NullCameraInput, UrlCameraInput, UrlCameraSpec, UsbCameraInput, UsbCameraSpec
|
||||
from .config import MotionAgentConfig
|
||||
from .recognizer import MediaPipeGestureRecognizer, NullGestureRecognizer
|
||||
|
||||
|
||||
class OutputClosed(Exception):
|
||||
"""Signal that the parent process stopped consuming worker events."""
|
||||
|
||||
|
||||
def _parse_indexes(raw: str | None, fallback: tuple[int, ...]) -> tuple[int, ...]:
|
||||
if raw is None:
|
||||
return fallback
|
||||
values = [item.strip() for item in raw.split(",") if item.strip()]
|
||||
return tuple(int(item) for item in values) or fallback
|
||||
|
||||
|
||||
def _parse_urls(raw: str | None, fallback: tuple[str, ...]) -> tuple[str, ...]:
|
||||
if raw is None:
|
||||
return fallback
|
||||
return tuple(item.strip() for item in raw.split(",") if item.strip())
|
||||
|
||||
|
||||
def _build_parser() -> argparse.ArgumentParser:
|
||||
parser = argparse.ArgumentParser(description="Run the Planet motion recognition worker.")
|
||||
parser.add_argument("--camera-indexes", default=None)
|
||||
parser.add_argument("--camera-urls", default=None)
|
||||
parser.add_argument(
|
||||
"--mode",
|
||||
choices=["auto", "single", "dual", "dual_redundant", "single_fallback", "calibrated_3d"],
|
||||
default=None,
|
||||
)
|
||||
parser.add_argument("--width", type=int, default=None)
|
||||
parser.add_argument("--height", type=int, default=None)
|
||||
parser.add_argument("--fps", type=int, default=None)
|
||||
parser.add_argument("--max-event-hz", type=int, default=None)
|
||||
parser.add_argument("--max-skeleton-hz", type=int, default=None)
|
||||
parser.add_argument("--dry-run", action="store_true")
|
||||
return parser
|
||||
|
||||
|
||||
def _config_from_args(argv: list[str] | None = None) -> MotionAgentConfig:
|
||||
args = _build_parser().parse_args(argv)
|
||||
config = MotionAgentConfig.from_env()
|
||||
return replace(
|
||||
config,
|
||||
camera_indexes=_parse_indexes(args.camera_indexes, config.camera_indexes),
|
||||
camera_urls=_parse_urls(args.camera_urls, config.camera_urls),
|
||||
camera_width=args.width or config.camera_width,
|
||||
camera_height=args.height or config.camera_height,
|
||||
camera_fps=args.fps or config.camera_fps,
|
||||
max_event_hz=args.max_event_hz or config.max_event_hz,
|
||||
max_skeleton_hz=args.max_skeleton_hz if args.max_skeleton_hz is not None else config.max_skeleton_hz,
|
||||
mode=args.mode or config.mode,
|
||||
dry_run=args.dry_run or config.dry_run,
|
||||
)
|
||||
|
||||
|
||||
def _build_cameras(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)
|
||||
]
|
||||
return [
|
||||
UsbCameraInput(
|
||||
UsbCameraSpec(
|
||||
index=index,
|
||||
width=config.camera_width,
|
||||
height=config.camera_height,
|
||||
fps=config.camera_fps,
|
||||
)
|
||||
)
|
||||
for index in config.camera_indexes
|
||||
]
|
||||
|
||||
|
||||
def _resolve_mode(config: MotionAgentConfig, camera_count: int) -> str:
|
||||
if config.mode == "dual":
|
||||
return "dual_redundant"
|
||||
if config.mode in {"single", "dual_redundant", "single_fallback", "calibrated_3d"}:
|
||||
return config.mode
|
||||
return "dual_redundant" if camera_count >= 2 else "single"
|
||||
|
||||
|
||||
def _emit(kind: str, payload: Any) -> None:
|
||||
try:
|
||||
print(
|
||||
json.dumps({"kind": kind, "payload": payload}, ensure_ascii=False, separators=(",", ":")),
|
||||
flush=True,
|
||||
)
|
||||
except BrokenPipeError as exc:
|
||||
# Prevent Python's interpreter shutdown flush from printing another
|
||||
# BrokenPipeError after the parent process closes the JSONL pipe.
|
||||
sys.stdout = None
|
||||
raise OutputClosed from exc
|
||||
|
||||
|
||||
class LatestFrameReader:
|
||||
"""Continuously read one camera and expose only the newest frame.
|
||||
|
||||
OpenCV and RTSP sources can buffer frames when recognition is slower than
|
||||
capture. The recognizer should never drain that backlog; it should always
|
||||
work on the latest frame so gesture latency stays bounded.
|
||||
"""
|
||||
|
||||
def __init__(self, camera: Any, *, fallback_fps: int) -> None:
|
||||
self.camera = camera
|
||||
self.camera_id = str(getattr(camera, "camera_id", "unknown"))
|
||||
self._fallback_interval = 1 / max(1, fallback_fps)
|
||||
self._lock = threading.Lock()
|
||||
self._stop = threading.Event()
|
||||
self._thread: threading.Thread | None = None
|
||||
self._frame: Any = None
|
||||
self._frame_seq = 0
|
||||
self._frame_at = 0.0
|
||||
self._error: str | None = None
|
||||
|
||||
def start(self) -> None:
|
||||
if self._thread and self._thread.is_alive():
|
||||
return
|
||||
self._stop.clear()
|
||||
self._thread = threading.Thread(target=self._run, name=f"motion-frame-reader-{self.camera_id}", daemon=True)
|
||||
self._thread.start()
|
||||
|
||||
def stop(self) -> None:
|
||||
self._stop.set()
|
||||
if self._thread and self._thread.is_alive():
|
||||
self._thread.join(timeout=1)
|
||||
self._thread = None
|
||||
|
||||
@property
|
||||
def frame_count(self) -> int:
|
||||
with self._lock:
|
||||
return self._frame_seq
|
||||
|
||||
@property
|
||||
def error(self) -> str | None:
|
||||
with self._lock:
|
||||
return self._error
|
||||
|
||||
def latest_after(self, last_seq: int) -> tuple[bool, int, Any, float]:
|
||||
with self._lock:
|
||||
if self._frame_seq <= last_seq:
|
||||
return False, last_seq, None, self._frame_at
|
||||
return True, self._frame_seq, self._frame, self._frame_at
|
||||
|
||||
def _run(self) -> None:
|
||||
while not self._stop.is_set():
|
||||
try:
|
||||
frame = self.camera.read()
|
||||
except Exception as exc:
|
||||
with self._lock:
|
||||
self._error = str(exc)
|
||||
return
|
||||
with self._lock:
|
||||
self._frame = frame
|
||||
self._frame_seq += 1
|
||||
self._frame_at = time.monotonic()
|
||||
self._error = None
|
||||
if frame is None:
|
||||
time.sleep(self._fallback_interval)
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
config = _config_from_args(argv)
|
||||
cameras = _build_cameras(config)
|
||||
recognizer = NullGestureRecognizer() if config.dry_run else MediaPipeGestureRecognizer()
|
||||
mode = _resolve_mode(config, len(cameras))
|
||||
min_interval = 1 / max(1, config.max_event_hz)
|
||||
skeleton_interval = 1 / config.max_skeleton_hz if config.max_skeleton_hz > 0 else None
|
||||
last_skeleton_at = 0.0
|
||||
opened: list[Any] = []
|
||||
readers: list[LatestFrameReader] = []
|
||||
last_frame_seq: dict[str, int] = {}
|
||||
last_stats_at = time.monotonic()
|
||||
last_frame_total = 0
|
||||
recognition_total = 0
|
||||
try:
|
||||
for camera in cameras:
|
||||
camera.open()
|
||||
opened.append(camera)
|
||||
reader = LatestFrameReader(camera, fallback_fps=config.camera_fps)
|
||||
reader.start()
|
||||
readers.append(reader)
|
||||
last_frame_seq[reader.camera_id] = 0
|
||||
_emit(
|
||||
"status",
|
||||
{
|
||||
"devices_open": True,
|
||||
"recognizer": recognizer.name,
|
||||
"error": None,
|
||||
"fps": 0.0,
|
||||
"recognition_fps": 0.0,
|
||||
},
|
||||
)
|
||||
while True:
|
||||
loop_started_at = time.monotonic()
|
||||
observations = []
|
||||
for reader in readers:
|
||||
error = reader.error
|
||||
if error:
|
||||
raise RuntimeError(error)
|
||||
has_frame, seq, frame, _frame_at = reader.latest_after(last_frame_seq.get(reader.camera_id, 0))
|
||||
if not has_frame:
|
||||
continue
|
||||
last_frame_seq[reader.camera_id] = seq
|
||||
observation = recognizer.recognize(frame)
|
||||
recognition_total += 1
|
||||
matched_gesture = None
|
||||
matched_confidence = 0.0
|
||||
if observation is not None:
|
||||
matched_gesture = observation.gesture
|
||||
matched_confidence = observation.confidence
|
||||
observations.append(
|
||||
{
|
||||
"gesture": observation.gesture,
|
||||
"confidence": observation.confidence,
|
||||
"intensity": observation.intensity,
|
||||
"timestamp_ms": observation.timestamp_ms,
|
||||
"camera_id": reader.camera_id,
|
||||
}
|
||||
)
|
||||
now = time.monotonic()
|
||||
if skeleton_interval is not None and now - last_skeleton_at >= skeleton_interval:
|
||||
skeleton = recognizer.debug_skeleton(
|
||||
frame,
|
||||
camera_id=reader.camera_id,
|
||||
mode=mode,
|
||||
matched_gesture=matched_gesture,
|
||||
confidence=matched_confidence,
|
||||
)
|
||||
if skeleton is not None:
|
||||
_emit("skeleton", skeleton.to_dict())
|
||||
last_skeleton_at = now
|
||||
if observations:
|
||||
_emit("observations", observations)
|
||||
now = time.monotonic()
|
||||
if now - last_stats_at >= 1:
|
||||
frame_total = sum(reader.frame_count for reader in readers)
|
||||
elapsed = max(0.001, now - last_stats_at)
|
||||
_emit(
|
||||
"status",
|
||||
{
|
||||
"devices_open": True,
|
||||
"recognizer": recognizer.name,
|
||||
"error": None,
|
||||
"fps": round((frame_total - last_frame_total) / elapsed, 2),
|
||||
"recognition_fps": round(recognition_total / elapsed, 2),
|
||||
},
|
||||
)
|
||||
last_stats_at = now
|
||||
last_frame_total = frame_total
|
||||
recognition_total = 0
|
||||
time.sleep(max(0.001, min_interval - (time.monotonic() - loop_started_at)))
|
||||
except (KeyboardInterrupt, OutputClosed):
|
||||
return 0
|
||||
except Exception as exc:
|
||||
_emit("status", {"devices_open": False, "recognizer": recognizer.name, "error": str(exc)})
|
||||
return 2
|
||||
finally:
|
||||
for reader in readers:
|
||||
reader.stop()
|
||||
for camera in opened:
|
||||
try:
|
||||
camera.close()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
_emit("status", {"devices_open": False, "recognizer": recognizer.name, "error": None})
|
||||
except OutputClosed:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main(sys.argv[1:]))
|
||||
Reference in New Issue
Block a user