infinicode 2.8.7 → 2.8.9

package/packages/robopark/scheduler/preview_agent.py

@@ -0,0 +1,639 @@
+#!/usr/bin/env python3
+"""
+RoboPark Preview Agent — runs on each robot/satellite Pi.
+
+Polls the scheduler for an active operator preview request. When active,
+opens the selected camera + microphone and publishes them to the returned
+LiveKit room. When the preview expires or is stopped, the agent leaves the
+room and releases the hardware.
+
+Configuration (env / ~/.robopark/preview_agent.json):
+  SCHEDULER_URL          base URL of the RoboPark scheduler
+  ROBOT_ID               robot identity in the scheduler (defaults to hostname)
+  DEVICE_TOKEN           long-lived device token (after enrollment)
+  ENROLLMENT_TOKEN       one-time enrollment token (device token will be saved)
+  VIDEO_DEVICE           camera path/index/name (e.g. /dev/video0, 0, "PiCamera")
+  AUDIO_DEVICE           microphone name/index or "default"
+  VIDEO_WIDTH / HEIGHT   capture resolution (default 640x480)
+  VIDEO_FPS              capture fps (default 15)
+  POLL_INTERVAL          seconds between scheduler polls (default 3)
+  HEARTBEAT_INTERVAL     seconds between device heartbeats (default 30)
+"""
+from __future__ import annotations
+
+import argparse
+import asyncio
+import json
+import logging
+import os
+import signal
+import sys
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Optional
+
+import httpx
+
+logger = logging.getLogger("robopark.preview_agent")
+
+CONFIG_DIR = Path.home() / ".robopark"
+CONFIG_FILE = CONFIG_DIR / "preview_agent.json"
+TOKEN_FILE = CONFIG_DIR / "device_token"
+
+DEFAULT_VIDEO_WIDTH = 640
+DEFAULT_VIDEO_HEIGHT = 480
+DEFAULT_FPS = 15
+DEFAULT_POLL_INTERVAL = 3.0
+DEFAULT_HEARTBEAT_INTERVAL = 30.0
+
+
+def _load_config() -> dict:
+    cfg: dict = {}
+    if CONFIG_FILE.exists():
+        try:
+            cfg = json.loads(CONFIG_FILE.read_text(encoding="utf8"))
+        except Exception as e:
+            logger.warning(f"failed to read {CONFIG_FILE}: {e}")
+    return cfg
+
+
+def _save_config(cfg: dict) -> None:
+    CONFIG_DIR.mkdir(parents=True, exist_ok=True)
+    CONFIG_FILE.write_text(json.dumps(cfg, indent=2), encoding="utf8")
+
+
+def _load_token() -> Optional[str]:
+    if TOKEN_FILE.exists():
+        return TOKEN_FILE.read_text(encoding="utf8").strip() or None
+    return None
+
+
+def _save_token(token: str) -> None:
+    CONFIG_DIR.mkdir(parents=True, exist_ok=True)
+    TOKEN_FILE.write_text(token, encoding="utf8")
+    os.chmod(TOKEN_FILE, 0o600)
+
+
+async def _enroll(scheduler_url: str, enrollment_token: str, robot_id: str) -> str:
+    """Enroll this Pi with the scheduler and return the device token."""
+    import socket
+    payload = {
+        "enrollment_token": enrollment_token,
+        "name": robot_id,
+        "lan_ip": _get_lan_ip(),
+    }
+    async with httpx.AsyncClient() as client:
+        r = await client.post(
+            f"{scheduler_url.rstrip('/')}/api/devices/enroll",
+            json=payload,
+            timeout=30.0,
+        )
+        r.raise_for_status()
+        data = r.json()
+    device_token = data.get("device_token")
+    if not device_token:
+        raise RuntimeError("enrollment did not return a device_token")
+    _save_token(device_token)
+    cfg = _load_config()
+    cfg["device_id"] = data.get("device_id")
+    cfg["scheduler_url"] = data.get("scheduler_url", scheduler_url)
+    _save_config(cfg)
+    logger.info(f"enrolled as device {data.get('device_id')}")
+    return device_token
+
+
+def _get_lan_ip() -> Optional[str]:
+    try:
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        s.settimeout(0.5)
+        s.connect(("10.255.255.255", 1))
+        ip = s.getsockname()[0]
+        s.close()
+        return ip
+    except Exception:
+        return None
+
+
+async def _send_heartbeat(scheduler_url: str, device_id: str, token: str) -> None:
+    try:
+        async with httpx.AsyncClient() as client:
+            await client.post(
+                f"{scheduler_url.rstrip('/')}/api/devices/{device_id}/heartbeat",
+                json={"status": "online"},
+                headers={"Authorization": f"Bearer {token}"},
+                timeout=10.0,
+            )
+    except Exception as e:
+        logger.debug(f"heartbeat failed: {e}")
+
+
+@dataclass
+class PreviewState:
+    active: bool = False
+    url: Optional[str] = None
+    token: Optional[str] = None
+    room: Optional[str] = None
+
+
+class PreviewAgent:
+    def __init__(self, cfg: dict):
+        self.scheduler_url = cfg.get("scheduler_url", os.getenv("SCHEDULER_URL", "http://localhost:8080"))
+        self.robot_id = cfg.get("robot_id", os.getenv("ROBOT_ID", _hostname()))
+        self.device_token: Optional[str] = cfg.get("device_token") or os.getenv("DEVICE_TOKEN") or _load_token()
+        self.device_id: Optional[str] = cfg.get("device_id")
+        self.video_device = cfg.get("video_device", os.getenv("VIDEO_DEVICE", "auto"))
+        self.audio_device = cfg.get("audio_device", os.getenv("AUDIO_DEVICE", "default"))
+        self.width = int(cfg.get("video_width", os.getenv("VIDEO_WIDTH", DEFAULT_VIDEO_WIDTH)))
+        self.height = int(cfg.get("video_height", os.getenv("VIDEO_HEIGHT", DEFAULT_VIDEO_HEIGHT)))
+        self.fps = int(cfg.get("video_fps", os.getenv("VIDEO_FPS", DEFAULT_FPS)))
+        self.poll_interval = float(cfg.get("poll_interval", os.getenv("POLL_INTERVAL", DEFAULT_POLL_INTERVAL)))
+        self.heartbeat_interval = float(cfg.get("heartbeat_interval", os.getenv("HEARTBEAT_INTERVAL", DEFAULT_HEARTBEAT_INTERVAL)))
+
+        self._shutdown = asyncio.Event()
+        self._task: Optional[asyncio.Task] = None
+        self._session: Optional[httpx.AsyncClient] = None
+        self._current_state: PreviewState = PreviewState()
+        self._publisher: Optional["LiveKitPublisher"] = None
+
+    async def run(self) -> None:
+        enrollment_token = os.getenv("ENROLLMENT_TOKEN") or cfg.get("enrollment_token")
+        if not self.device_token and enrollment_token:
+            self.device_token = await _enroll(self.scheduler_url, enrollment_token, self.robot_id)
+
+        if not self.device_token:
+            logger.error("no DEVICE_TOKEN and no ENROLLMENT_TOKEN; cannot poll scheduler")
+            sys.exit(1)
+
+        if not self.device_id:
+            await self._resolve_device_id()
+
+        self._session = httpx.AsyncClient()
+
+        tasks = [
+            asyncio.create_task(self._poll_loop()),
+            asyncio.create_task(self._heartbeat_loop()),
+        ]
+        await self._shutdown.wait()
+        for t in tasks:
+            t.cancel()
+            try:
+                await t
+            except asyncio.CancelledError:
+                pass
+        await self._stop_publisher()
+        await self._session.aclose()
+
+    async def _resolve_device_id(self) -> None:
+        """Look up our device_id from the scheduler using the token."""
+        if not self._session or not self.device_token:
+            return
+        try:
+            r = await self._session.get(
+                f"{self.scheduler_url.rstrip('/')}/api/devices",
+                headers={"Authorization": f"Bearer {self.device_token}"},
+                timeout=10.0,
+            )
+            r.raise_for_status()
+            for d in r.json():
+                if d.get("name") == self.robot_id:
+                    self.device_id = d.get("id")
+                    cfg = _load_config()
+                    cfg["device_id"] = self.device_id
+                    _save_config(cfg)
+                    return
+        except Exception as e:
+            logger.warning(f"could not resolve device_id: {e}")
+
+    async def _poll_loop(self) -> None:
+        while not self._shutdown.is_set():
+            try:
+                state = await self._fetch_preview_state()
+                await self._apply_state(state)
+            except Exception as e:
+                logger.warning(f"poll error: {e}")
+            try:
+                await asyncio.wait_for(self._shutdown.wait(), timeout=self.poll_interval)
+            except asyncio.TimeoutError:
+                pass
+
+    async def _heartbeat_loop(self) -> None:
+        while not self._shutdown.is_set():
+            if self.device_id and self.device_token:
+                await _send_heartbeat(self.scheduler_url, self.device_id, self.device_token)
+            try:
+                await asyncio.wait_for(self._shutdown.wait(), timeout=self.heartbeat_interval)
+            except asyncio.TimeoutError:
+                pass
+
+    async def _fetch_preview_state(self) -> PreviewState:
+        if not self._session or not self.device_token:
+            return PreviewState()
+        r = await self._session.get(
+            f"{self.scheduler_url.rstrip('/')}/api/robots/{self.robot_id}/preview/agent",
+            headers={"Authorization": f"Bearer {self.device_token}"},
+            timeout=10.0,
+        )
+        r.raise_for_status()
+        data = r.json()
+        if not data.get("active"):
+            return PreviewState()
+        return PreviewState(
+            active=True,
+            url=data.get("url"),
+            token=data.get("token"),
+            room=data.get("room"),
+        )
+
+    async def _apply_state(self, state: PreviewState) -> None:
+        same = (
+            state.active == self._current_state.active
+            and state.room == self._current_state.room
+            and state.token == self._current_state.token
+            and state.url == self._current_state.url
+        )
+        if same:
+            return
+        self._current_state = state
+        if not state.active:
+            await self._stop_publisher()
+            return
+        await self._start_publisher(state)
+
+    async def _start_publisher(self, state: PreviewState) -> None:
+        await self._stop_publisher()
+        if not state.url or not state.token or not state.room:
+            return
+        try:
+            pub = LiveKitPublisher(state.url, state.token, state.room, self)
+            await pub.start()
+            self._publisher = pub
+            logger.info(f"joined preview room {state.room}")
+        except Exception as e:
+            logger.error(f"failed to start publisher: {e}")
+
+    async def _stop_publisher(self) -> None:
+        if self._publisher:
+            try:
+                await self._publisher.stop()
+            except Exception as e:
+                logger.warning(f"publisher stop error: {e}")
+            self._publisher = None
+
+    def shutdown(self) -> None:
+        self._shutdown.set()
+
+
+class LiveKitPublisher:
+    """Encapsulates LiveKit room connection, camera capture and mic capture."""
+
+    def __init__(self, url: str, token: str, room_name: str, agent: PreviewAgent):
+        from livekit import rtc
+        self.url = url
+        self.token = token
+        self.room_name = room_name
+        self.agent = agent
+        self.rtc = rtc
+        self.room: Optional[rtc.Room] = None
+        self.video_source: Optional[rtc.VideoSource] = None
+        self.video_track: Optional[rtc.LocalVideoTrack] = None
+        self.audio_source: Optional[rtc.AudioSource] = None
+        self.audio_track: Optional[rtc.LocalAudioTrack] = None
+        self._stop_event = asyncio.Event()
+        self._tasks: list[asyncio.Task] = []
+        self._capture: Optional["VideoCapture"] = None
+
+    async def start(self) -> None:
+        self.room = self.rtc.Room()
+        await self.room.connect(self.url, self.token)
+
+        # Video
+        self.video_source = self.rtc.VideoSource(self.agent.width, self.agent.height)
+        self.video_track = self.rtc.LocalVideoTrack.create_video_track("camera", self.video_source)
+        vopts = self.rtc.TrackPublishOptions()
+        vopts.source = self.rtc.TrackSource.SOURCE_CAMERA
+        await self.room.local_participant.publish_track(self.video_track, vopts)
+
+        # Audio
+        self.audio_source = self.rtc.AudioSource(48000, 1)
+        self.audio_track = self.rtc.LocalAudioTrack.create_audio_track("microphone", self.audio_source)
+        aopts = self.rtc.TrackPublishOptions()
+        aopts.source = self.rtc.TrackSource.SOURCE_MICROPHONE
+        await self.room.local_participant.publish_track(self.audio_track, aopts)
+
+        self._capture = create_video_capture(self.agent.video_device, self.agent.width, self.agent.height, self.agent.fps)
+        if self._capture:
+            self._tasks.append(asyncio.create_task(self._video_loop()))
+        if has_audio():
+            self._tasks.append(asyncio.create_task(self._audio_loop()))
+
+    async def stop(self) -> None:
+        self._stop_event.set()
+        for t in self._tasks:
+            t.cancel()
+            try:
+                await t
+            except asyncio.CancelledError:
+                pass
+        self._tasks.clear()
+        if self._capture:
+            self._capture.stop()
+            self._capture = None
+        if self.room:
+            await self.room.disconnect()
+            self.room = None
+
+    async def _video_loop(self) -> None:
+        assert self._capture is not None and self.video_source is not None
+        frame_interval = 1.0 / self.agent.fps
+        while not self._stop_event.is_set():
+            start = time.monotonic()
+            frame = self._capture.read()
+            if frame is not None:
+                self.video_source.capture_frame(frame)
+            elapsed = time.monotonic() - start
+            sleep_for = frame_interval - elapsed
+            if sleep_for > 0:
+                try:
+                    await asyncio.wait_for(self._stop_event.wait(), timeout=sleep_for)
+                except asyncio.TimeoutError:
+                    pass
+
+    async def _audio_loop(self) -> None:
+        assert self.audio_source is not None
+        mic = create_audio_capture(self.agent.audio_device)
+        if mic is None:
+            return
+        frame_duration = 0.02  # 20ms
+        samples_per_frame = int(48000 * frame_duration)
+        while not self._stop_event.is_set():
+            frame = mic.read(samples_per_frame)
+            if frame is not None:
+                self.audio_source.capture_frame(frame)
+            try:
+                await asyncio.wait_for(self._stop_event.wait(), timeout=frame_duration)
+            except asyncio.TimeoutError:
+                pass
+
+
+# -----------------------------------------------------------------------------
+# Video capture abstraction: V4L2/USB via OpenCV, or picamera2 on Pi.
+# -----------------------------------------------------------------------------
+
+class VideoCapture:
+    def read(self) -> Optional["rtc.VideoFrame"]:
+        raise NotImplementedError
+
+    def stop(self) -> None:
+        raise NotImplementedError
+
+
+class OpencvVideoCapture(VideoCapture):
+    def __init__(self, device: int | str, width: int, height: int, fps: int):
+        import cv2
+        self.cv2 = cv2
+        if isinstance(device, str) and device.startswith("/dev/video"):
+            device = int(device.replace("/dev/video", ""))
+        self.cap = cv2.VideoCapture(device)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
+        self.cap.set(cv2.CAP_PROP_FPS, fps)
+        self._ensure_frame()
+
+    def _ensure_frame(self):
+        ok, _ = self.cap.read()
+        if not ok:
+            raise RuntimeError("cannot read from camera")
+
+    def read(self):
+        from livekit import rtc
+        ok, bgr = self.cap.read()
+        if not ok or bgr is None:
+            return None
+        return rtc.VideoFrame(
+            width=bgr.shape[1],
+            height=bgr.shape[0],
+            type=rtc.VideoBufferType.BGR,
+            data=bgr.tobytes(),
+        )
+
+    def stop(self):
+        self.cap.release()
+
+
+class Picamera2Capture(VideoCapture):
+    def __init__(self, width: int, height: int, fps: int):
+        from picamera2 import Picamera2
+        self.picam = Picamera2()
+        config = self.picam.create_video_configuration(
+            main={"format": "RGB888", "size": (width, height)},
+            controls={"FrameRate": fps},
+        )
+        self.picam.configure(config)
+        self.picam.start()
+        self.width = width
+        self.height = height
+
+    def read(self):
+        from livekit import rtc
+        arr = self.picam.capture_array()
+        if arr is None:
+            return None
+        return rtc.VideoFrame(
+            width=self.width,
+            height=self.height,
+            type=rtc.VideoBufferType.RGB,
+            data=arr.tobytes(),
+        )
+
+    def stop(self):
+        try:
+            self.picam.stop()
+        except Exception:
+            pass
+
+
+def create_video_capture(device: str, width: int, height: int, fps: int) -> Optional[VideoCapture]:
+    if device.lower() in ("none", "", "false", "null"):
+        return None
+    try:
+        from livekit import rtc
+        _ = rtc.VideoSource
+    except Exception as e:
+        logger.error(f"livekit python sdk not installed: {e}")
+        return None
+
+    # Auto-detect: prefer first V4L2 device, fall back to picamera2 if available.
+    if device.lower() in ("auto", "default", "first"):
+        for i in range(4):
+            try:
+                cap = OpencvVideoCapture(i, width, height, fps)
+                logger.info(f"auto-selected USB camera /dev/video{i}")
+                return cap
+            except Exception:
+                continue
+        try:
+            cap = Picamera2Capture(width, height, fps)
+            logger.info("auto-selected Pi Camera")
+            return cap
+        except Exception:
+            pass
+        logger.error("no camera found (tried V4L2 and picamera2)")
+        return None
+
+    if device.lower() in ("picamera", "picamera2", "pi", "rpi"):
+        return Picamera2Capture(width, height, fps)
+
+    # Treat as V4L2 index or path
+    return OpencvVideoCapture(device, width, height, fps)
+
+
+# -----------------------------------------------------------------------------
+# Audio capture abstraction: PyAudio or sounddevice -> LiveKit AudioFrame.
+# -----------------------------------------------------------------------------
+
+class AudioCapture:
+    def read(self, samples_per_frame: int) -> Optional["rtc.AudioFrame"]:
+        raise NotImplementedError
+
+    def stop(self) -> None:
+        raise NotImplementedError
+
+
+class PyAudioCapture(AudioCapture):
+    def __init__(self, device: str | int | None):
+        import pyaudio
+        self.pa = pyaudio.PyAudio()
+        self.device_index = self._resolve_device(device)
+        self.buffer = bytearray()
+        self.stream = self.pa.open(
+            format=pyaudio.paInt16,
+            channels=1,
+            rate=48000,
+            input=True,
+            input_device_index=self.device_index,
+            frames_per_buffer=960,
+            stream_callback=self._callback,
+        )
+        self.stream.start_stream()
+
+    def _resolve_device(self, device: str | int | None) -> Optional[int]:
+        if device is None or device == "default":
+            return None
+        if isinstance(device, int):
+            return device
+        # Try exact name match
+        for i in range(self.pa.get_device_count()):
+            info = self.pa.get_device_info_by_index(i)
+            if info.get("maxInputChannels", 0) > 0 and device.lower() in str(info.get("name", "")).lower():
+                return i
+        return None
+
+    def _callback(self, in_data, frame_count, time_info, status):
+        self.buffer.extend(in_data)
+        return (None, pyaudio.paContinue)
+
+    def read(self, samples_per_frame: int):
+        from livekit import rtc
+        bytes_needed = samples_per_frame * 2  # int16 mono
+        while len(self.buffer) < bytes_needed:
+            time.sleep(0.005)
+        chunk = bytes(self.buffer[:bytes_needed])
+        self.buffer = self.buffer[bytes_needed:]
+        return rtc.AudioFrame(
+            data=chunk,
+            sample_rate=48000,
+            num_channels=1,
+            samples_per_channel=samples_per_frame,
+        )
+
+    def stop(self):
+        if self.stream:
+            self.stream.stop_stream()
+            self.stream.close()
+        self.pa.terminate()
+
+
+def has_audio() -> bool:
+    try:
+        import pyaudio  # noqa: F401
+        return True
+    except Exception:
+        return False
+
+
+def create_audio_capture(device: str) -> Optional[AudioCapture]:
+    if device.lower() in ("none", "", "false", "null"):
+        return None
+    try:
+        import pyaudio  # noqa: F401
+        return PyAudioCapture(device if device != "default" else None)
+    except Exception as e:
+        logger.warning(f"pyaudio not available: {e}")
+        return None
+
+
+def _hostname() -> str:
+    import socket
+    return socket.gethostname().split(".")[0]
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="RoboPark preview agent")
+    parser.add_argument("--scheduler-url", default=os.getenv("SCHEDULER_URL", "http://localhost:8080"))
+    parser.add_argument("--robot-id", default=os.getenv("ROBOT_ID", _hostname()))
+    parser.add_argument("--device-token", default=os.getenv("DEVICE_TOKEN"))
+    parser.add_argument("--enrollment-token", default=os.getenv("ENROLLMENT_TOKEN"))
+    parser.add_argument("--video-device", default=os.getenv("VIDEO_DEVICE", "auto"))
+    parser.add_argument("--audio-device", default=os.getenv("AUDIO_DEVICE", "default"))
+    parser.add_argument("--width", type=int, default=int(os.getenv("VIDEO_WIDTH", DEFAULT_VIDEO_WIDTH)))
+    parser.add_argument("--height", type=int, default=int(os.getenv("VIDEO_HEIGHT", DEFAULT_VIDEO_HEIGHT)))
+    parser.add_argument("--fps", type=int, default=int(os.getenv("VIDEO_FPS", DEFAULT_FPS)))
+    parser.add_argument("--poll-interval", type=float, default=float(os.getenv("POLL_INTERVAL", DEFAULT_POLL_INTERVAL)))
+    parser.add_argument("--heartbeat-interval", type=float, default=float(os.getenv("HEARTBEAT_INTERVAL", DEFAULT_HEARTBEAT_INTERVAL)))
+    parser.add_argument("--save-config", action="store_true", help="write CLI args to ~/.robopark/preview_agent.json")
+    parser.add_argument("-v", "--verbose", action="store_true")
+    args = parser.parse_args()
+
+    logging.basicConfig(
+        level=logging.DEBUG if args.verbose else logging.INFO,
+        format="%(asctime)s %(levelname)s %(name)s: %(message)s",
+    )
+
+    cfg = _load_config()
+    cfg.update({
+        "scheduler_url": args.scheduler_url,
+        "robot_id": args.robot_id,
+        "video_device": args.video_device,
+        "audio_device": args.audio_device,
+        "video_width": args.width,
+        "video_height": args.height,
+        "video_fps": args.fps,
+        "poll_interval": args.poll_interval,
+        "heartbeat_interval": args.heartbeat_interval,
+    })
+    if args.device_token:
+        cfg["device_token"] = args.device_token
+        _save_token(args.device_token)
+    if args.enrollment_token:
+        cfg["enrollment_token"] = args.enrollment_token
+
+    if args.save_config:
+        _save_config(cfg)
+        logger.info(f"saved config to {CONFIG_FILE}")
+
+    agent = PreviewAgent(cfg)
+
+    loop = asyncio.new_event_loop()
+    asyncio.set_event_loop(loop)
+    for sig in (signal.SIGINT, signal.SIGTERM):
+        loop.add_signal_handler(sig, agent.shutdown)
+
+    try:
+        loop.run_until_complete(agent.run())
+    finally:
+        loop.close()
+
+
+if __name__ == "__main__":
+    main()

package/packages/robopark/scheduler/requirements.txt

@@ -5,4 +5,8 @@ httpx==0.27.2
 pydantic==2.9.2
 python-multipart==0.0.12
 livekit-api>=0.10
+livekit>=0.18
+opencv-python>=4.8
+pyaudio>=0.2
+picamera2>=0.3; platform_machine == 'aarch64' or platform_machine == 'arm64'
 PyJWT==2.9.0