infinicode 2.8.13 → 2.8.15

package/packages/robopark/pi-client/livekit_bridge.py

@@ -0,0 +1,289 @@
+"""LiveKit bridge for the RoboPark Pi client.
+
+Joins a LiveKit room as the Pi's device, publishes mic + camera, and forwards
+inbound data-channel messages to MotorBridge.handle_command().
+
+Token strategy: the Pi doesn't hold LiveKit API credentials. It asks the
+scheduler (POST /api/livekit/token) for a short-lived JWT signed by the
+scheduler. The Pi re-fetches the token whenever it's about to (re)connect.
+
+Install on Pi:
+    pip install livekit livekit-api sounddevice numpy av opencv-python-headless
+"""
+
+import asyncio
+import json
+import logging
+from typing import Callable, Awaitable, Optional
+
+import httpx
+
+log = logging.getLogger("robopark-pi.livekit")
+
+
+async def _fetch_livekit_token(
+    scheduler_url: str, device_id: str, device_token: str,
+    room: str, identity: str, ttl_seconds: int = 3600,
+) -> tuple[str, str]:
+    """Ask the scheduler for a LiveKit access token. Returns (url, token)."""
+    async with httpx.AsyncClient(timeout=10.0) as c:
+        r = await c.post(
+            f"{scheduler_url.rstrip('/')}/api/livekit/token",
+            json={"room": room, "identity": identity, "name": device_id,
+                  "can_publish": True, "can_subscribe": True,
+                  "ttl_seconds": ttl_seconds},
+        )
+        r.raise_for_status()
+        data = r.json()
+    return data["url"], data["token"]
+
+
+async def _connect_with_retry(
+    scheduler_url: str, device_id: str, device_token: str,
+    livekit_url: Optional[str], room_name: str, identity: str,
+    stop_event: asyncio.Event,
+) -> Optional["rtc.Room"]:
+    """Fetch a token and connect. Reconnect on failure until stopped."""
+    from livekit import rtc
+    backoff = 1.0
+    while not stop_event.is_set():
+        try:
+            url, token = await _fetch_livekit_token(
+                scheduler_url, device_id, device_token, room_name, identity,
+            )
+            url = livekit_url or url  # CLI override wins
+            room = rtc.Room()
+            log.info(f"Connecting to LiveKit {url} room={room_name} as {identity}")
+            await room.connect(url, token)
+            log.info(f"Connected to room {room_name}")
+            return room
+        except Exception as e:
+            log.warning(f"LiveKit connect failed: {e}; retrying in {backoff:.1f}s")
+            try:
+                await asyncio.wait_for(stop_event.wait(), timeout=backoff)
+                return None  # stopped while waiting
+            except asyncio.TimeoutError:
+                pass
+            backoff = min(backoff * 2, 30.0)
+    return None
+
+
+async def run_livekit(
+    livekit_url: Optional[str],
+    room_name: str,
+    identity: str,
+    on_motor_command: Callable[[str | bytes], Awaitable[str]],
+    stop_event: asyncio.Event,
+    production_mode_provider: Callable[[], bool],
+    scheduler_url: str = "",
+    device_id: str = "",
+    device_token: str = "",
+):
+    """Join a LiveKit room, publish mic + cam, subscribe to data channel.
+
+    Reconnects on disconnect, refetching the token each time.
+    Stops cleanly when production_mode flips False or stop_event is set.
+    """
+    try:
+        from livekit import rtc
+    except ImportError as e:
+        log.warning(f"LiveKit SDK not available, cannot join room: {e}")
+        return
+
+    while not stop_event.is_set():
+        if not production_mode_provider():
+            log.info("production_mode is OFF; not joining LiveKit")
+            try:
+                await asyncio.wait_for(stop_event.wait(), timeout=5.0)
+            except asyncio.TimeoutError:
+                continue
+            continue
+
+        room = await _connect_with_retry(
+            scheduler_url, device_id, device_token,
+            livekit_url, room_name, identity, stop_event,
+        )
+        if room is None:
+            return  # stop requested
+
+        @room.on("data_received")
+        def _on_data(packet):
+            asyncio.create_task(on_motor_command(packet.data))
+
+        @room.on("disconnected")
+        def _on_disc():
+            log.warning("LiveKit disconnected")
+
+        # --- Audio output: play remote participants' TTS through speakers ---
+        async def _play_remote_audio():
+            """Subscribe to audio tracks from other participants (agent TTS)
+            and play them through the Pi's default audio output device."""
+            try:
+                import sounddevice as sd
+                import numpy as np
+            except Exception as e:
+                log.warning(f"sounddevice/numpy unavailable for playback: {e}")
+                return
+
+            OUT_RATE = 48000  # sounddevice default output rate
+            _active_streams: dict = {}  # sid -> sd.OutputStream
+
+            def _on_track(track, publication, participant):
+                if track.kind != rtc.TrackKind.KIND_AUDIO:
+                    return
+                if participant.identity == room.local_participant.identity:
+                    return  # don't play our own mic
+                sid = publication.sid
+                log.info(f"audio out: subscribing to {participant.identity} / {sid}")
+
+                async def _stream_audio():
+                    try:
+                        stream = rtc.AudioStream(track=track)
+                        # Open output stream at 48kHz mono (Pi speaker)
+                        out = sd.OutputStream(samplerate=OUT_RATE, channels=1, dtype="int16", blocksize=0)
+                        out.start()
+                        _active_streams[sid] = out
+
+                        async for frame in stream:
+                            # SDK v1.1.12 yields AudioFrameEvent; v1.1.13 yields AudioFrame
+                            af = frame.frame if hasattr(frame, 'frame') else frame
+                            in_rate = af.sample_rate
+                            in_data = np.frombuffer(af.data, dtype=np.int16)
+                            if in_rate == OUT_RATE:
+                                out_data = in_data
+                            else:
+                                # Simple linear resample
+                                ratio = OUT_RATE / in_rate
+                                n_out = int(len(in_data) * ratio)
+                                indices = np.linspace(0, len(in_data) - 1, n_out)
+                                out_data = np.interp(indices, np.arange(len(in_data)), in_data.astype(np.float64)).astype(np.int16)
+
+                            try:
+                                out.write(out_data)
+                            except Exception as e:
+                                log.debug(f"audio out write error: {e}")
+                                break
+                    except Exception as e:
+                        log.warning(f"audio out stream error: {e}")
+                    finally:
+                        try:
+                            if sid in _active_streams:
+                                _active_streams[sid].stop()
+                                del _active_streams[sid]
+                        except Exception:
+                            pass
+                    log.info(f"audio out: stopped {sid}")
+
+                asyncio.create_task(_stream_audio())
+
+            @room.on("track_subscribed")
+            def _on_track_subscribed(track, publication, participant):
+                _on_track(track, publication, participant)
+
+            # Also handle tracks already present (agent joined before us)
+            for p in room.remote_participants.values():
+                for pub in p.track_publications.values():
+                    if pub.track and pub.track.kind == rtc.TrackKind.KIND_AUDIO:
+                        _on_track(pub.track, pub, p)
+
+            # Keep alive until disconnect
+            while not stop_event.is_set() and room.connection_state == rtc.ConnectionState.CONN_CONNECTED:
+                await asyncio.sleep(0.5)
+
+            # Cleanup
+            for s in _active_streams.values():
+                try: s.stop()
+                except Exception: pass
+            _active_streams.clear()
+
+        # Publish tracks
+        async def _publish_mic():
+            try:
+                import sounddevice as sd
+            except Exception as e:
+                log.warning(f"sounddevice unavailable, skipping mic: {e}")
+                return
+            # The USB mic is ALSA device index 1 (1 in, 0 out). Default is output-only.
+            MIC_DEVICE = 1
+            try:
+                log.info(f"mic: opening device {MIC_DEVICE} ({sd.query_devices()[MIC_DEVICE]['name']})")
+            except Exception:
+                log.warning(f"mic: device {MIC_DEVICE} not found, trying default")
+                MIC_DEVICE = None
+            try:
+                source = rtc.AudioSource(sample_rate=16000, num_channels=1)
+                track = rtc.LocalAudioTrack.create_audio_track("mic", source)
+                opts = rtc.TrackPublishOptions(source=rtc.TrackSource.SOURCE_MICROPHONE)
+                await room.local_participant.publish_track(track, opts)
+                log.info("mic published (source=MICROPHONE, 16kHz)")
+                loop = asyncio.get_event_loop()
+                def _cb(indata, frames, time_info, status):
+                    if status: log.debug(f"mic status: {status}")
+                    try:
+                        frame = rtc.AudioFrame(
+                            data=indata.tobytes(),
+                            sample_rate=16000,
+                            num_channels=1,
+                            samples_per_channel=frames,
+                        )
+                        asyncio.run_coroutine_threadsafe(
+                            source.capture_frame(frame), loop)
+                    except Exception: pass
+                with sd.InputStream(device=MIC_DEVICE, samplerate=16000, channels=1, dtype="int16", callback=_cb):
+                    while not stop_event.is_set() and room.connection_state == rtc.ConnectionState.CONN_CONNECTED:
+                        await asyncio.sleep(0.1)
+            except Exception as e:
+                log.warning(f"mic publish error: {e}")
+
+        async def _publish_cam():
+            try:
+                import cv2
+            except Exception as e:
+                log.warning(f"opencv unavailable, skipping camera: {e}")
+                return
+            cap = cv2.VideoCapture(0, cv2.CAP_V4L2)
+            if not cap.isOpened():
+                log.warning("camera /dev/video0 not available")
+                return
+            cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
+            cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
+            cap.set(cv2.CAP_PROP_FPS, 15)
+            try:
+                source = rtc.VideoSource(width=640, height=480)
+                track = rtc.LocalVideoTrack.create_video_track("cam", source)
+                opts = rtc.TrackPublishOptions(source=rtc.TrackSource.SOURCE_CAMERA)
+                await room.local_participant.publish_track(track, opts)
+                log.info("camera published (source=CAMERA)")
+                while not stop_event.is_set() and room.connection_state == rtc.ConnectionState.CONN_CONNECTED:
+                    ok, frame = cap.read()
+                    if not ok: await asyncio.sleep(0.1); continue
+                    rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+                    vf = rtc.VideoFrame(width=640, height=480, type=rtc.VideoBufferType.RGB24, data=rgb.tobytes())
+                    source.capture_frame(vf)
+                    await asyncio.sleep(1/15)
+            except Exception as e:
+                log.warning(f"camera publish error: {e}")
+            finally:
+                cap.release()
+
+        mic_task = asyncio.create_task(_publish_mic())
+        cam_task = asyncio.create_task(_publish_cam())
+        audio_out_task = asyncio.create_task(_play_remote_audio())
+
+        try:
+            while not stop_event.is_set():
+                if not production_mode_provider():
+                    log.info("production_mode flipped OFF; leaving room")
+                    break
+                if room.connection_state != rtc.ConnectionState.CONN_CONNECTED:
+                    log.info("room not connected; will reconnect")
+                    break
+                await asyncio.sleep(1)
+        finally:
+            mic_task.cancel(); cam_task.cancel(); audio_out_task.cancel()
+            for t in (mic_task, cam_task, audio_out_task):
+                try: await t
+                except Exception: pass
+            try: await room.disconnect()
+            except Exception: pass
+            log.info("left room, will loop to reconnect")

package/packages/robopark/pi-client/motor_bridge.py

@@ -0,0 +1,82 @@
+"""Motor bridge for RoboPark Pi client.
+
+Translates motor commands (currently delivered via LiveKit data channel
+from the agent) into HTTP POSTs against the local RoboVisionAI_PI motor
+server (same endpoints the agent already calls: /trigger-motor and
+/stop-motors).
+
+Command wire format (JSON on the data channel):
+    {"op": "motor", "name": "arm", "seconds": 3}
+    {"op": "stop"}
+"""
+
+import asyncio
+import json
+import logging
+from typing import Awaitable, Callable, Optional
+
+import httpx
+
+log = logging.getLogger("robopark-pi.motor")
+
+
+class MotorBridge:
+    def __init__(self, motor_server_url: Optional[str], dry_run: bool = False):
+        self.motor_server_url = (motor_server_url or "").rstrip("/")
+        self.dry_run = dry_run
+        self._lock = asyncio.Lock()  # the local motor server allows one motor at a time
+
+    async def handle_command(self, raw: str | bytes) -> str:
+        """Handle one command from the data channel. Returns a short result string."""
+        try:
+            text = raw.decode() if isinstance(raw, (bytes, bytearray)) else raw
+            msg = json.loads(text)
+        except Exception as e:
+            log.warning(f"Could not parse motor command {raw!r}: {e}")
+            return f"error: invalid json ({e})"
+
+        op = (msg.get("op") or "").lower()
+        if op == "motor":
+            name = msg.get("name")
+            seconds = int(msg.get("seconds", 3))
+            if not name:
+                return "error: missing 'name'"
+            seconds = max(1, min(seconds, 30))
+            return await self.move(name, seconds)
+        elif op == "stop":
+            return await self.stop_all()
+        elif op == "ping":
+            return "pong"
+        else:
+            log.warning(f"Unknown motor op: {op!r}")
+            return f"error: unknown op {op!r}"
+
+    async def move(self, name: str, seconds: int) -> str:
+        if self.dry_run or not self.motor_server_url:
+            log.info(f"[DRY-RUN] would move motor '{name}' for {seconds}s")
+            return f"dry-run: {name} {seconds}s"
+        async with self._lock:
+            try:
+                async with httpx.AsyncClient(timeout=seconds + 5.0) as c:
+                    r = await c.post(
+                        f"{self.motor_server_url}/trigger-motor",
+                        json={"motor_name": name, "seconds": seconds},
+                    )
+                if r.status_code == 200:
+                    return f"moved {name} for {seconds}s"
+                return f"motor server {r.status_code}: {r.text[:200]}"
+            except Exception as e:
+                log.error(f"move({name}) failed: {e}")
+                return f"error: {e}"
+
+    async def stop_all(self) -> str:
+        if self.dry_run or not self.motor_server_url:
+            log.info("[DRY-RUN] would stop all motors")
+            return "dry-run: stop"
+        try:
+            async with httpx.AsyncClient(timeout=5.0) as c:
+                r = await c.post(f"{self.motor_server_url}/stop-motors", json={})
+            return f"stop {r.status_code}"
+        except Exception as e:
+            log.error(f"stop_all failed: {e}")
+            return f"error: {e}"