hermes-bot/ros/hermes_mcp/hermes_mcp/bridge.py

"""The MCP side of the bridge: the tools, bearer-token auth and connection tracking. No ROS imports,
so it is testable with a stub camera.

MCP tool names are the stable contract with Hermes and must not change when the internals do:
  look()                  -> the next fresh JPEG from the camera
  turn_head(pan, tilt)    -> only registered if a head backend is given (servos fitted)
  head_position()         -> likewise
"""
from __future__ import annotations

import hmac
import threading
import time
from typing import Callable, Optional, Protocol

from mcp.server.mcpserver import Image, MCPServer
from mcp.server.transport_security import TransportSecuritySettings


class LatestFrame:
    """The newest camera frame, and a way to wait for one taken after a given moment: look() must
    return what the camera sees now, not a frame that was already sitting there."""

    def __init__(self) -> None:
        self._cv = threading.Condition()
        self._jpeg: Optional[bytes] = None
        self._t = -1.0

    def put(self, jpeg: bytes, t: Optional[float] = None) -> None:
        with self._cv:
            self._jpeg, self._t = jpeg, time.monotonic() if t is None else t
            self._cv.notify_all()

    def wait_newer(self, t0: float, timeout: float) -> Optional[bytes]:
        """The first frame received after t0, or None if none arrives within timeout seconds."""
        deadline = time.monotonic() + timeout
        with self._cv:
            while self._jpeg is None or self._t <= t0:
                remaining = deadline - time.monotonic()
                if remaining <= 0:
                    return None
                self._cv.wait(remaining)
            return self._jpeg


class Head(Protocol):
    def move(self, pan: float, tilt: float) -> tuple[float, float]: ...
    def position(self) -> tuple[float, float]: ...


def build_mcp(frames: LatestFrame, announce_look: Callable[[], None] = lambda: None,
              head: Optional[Head] = None, look_timeout_s: float = 3.0) -> MCPServer:
    mcp = MCPServer(
        "robot",
        instructions=(
            "This server is a small physical robot on the user's desk. "
            "Call look() whenever you need to see what is in front of it; the image is current. "
            "If turn_head is available, aim first and then look."
        ),
    )

    @mcp.tool()
    def look() -> Image:
        """Take a photograph with the robot's camera and return it. Use this to see the room,
        the desk, the person, or anything the user asks you to look at."""
        t0 = time.monotonic()
        announce_look()                      # the face squints and flashes
        jpeg = frames.wait_newer(t0, look_timeout_s)
        if jpeg is None:
            raise RuntimeError("the robot's camera is not producing images")
        return Image(data=jpeg, format="jpeg")

    if head is not None:
        @mcp.tool()
        def turn_head(pan: float = 0.0, tilt: float = 0.0) -> str:
            """Aim the robot's head (and so its camera). Angles are in degrees from straight ahead.
            pan: positive turns to the robot's left, negative to its right.
            tilt: positive looks up, negative looks down.
            Values beyond the mechanical limits are clamped. Call look() afterwards to see."""
            p, t = head.move(pan, tilt)
            return f"head now at pan {p:+.0f} degrees, tilt {t:+.0f} degrees"

        @mcp.tool()
        def head_position() -> str:
            """Report where the robot's head is currently pointing."""
            p, t = head.position()
            return f"pan {p:+.0f} degrees, tilt {t:+.0f} degrees"

    return mcp


class BearerAuth:
    """ASGI middleware: reject any HTTP request without the right bearer token. Lifespan events pass
    straight through so the MCP session manager starts properly."""

    def __init__(self, app, token: str) -> None:
        self.app = app
        self.expected = f"Bearer {token}".encode()

    async def __call__(self, scope, receive, send):
        if scope["type"] == "http":
            supplied = dict(scope.get("headers") or []).get(b"authorization", b"")
            if not hmac.compare_digest(supplied, self.expected):
                await send({"type": "http.response.start", "status": 401,
                            "headers": [(b"content-type", b"text/plain"), (b"www-authenticate", b"Bearer")]})
                await send({"type": "http.response.body", "body": b"unauthorized\n"})
                return
        await self.app(scope, receive, send)


class Activity:
    """ASGI middleware, placed inside BearerAuth so that only authenticated requests count: tracks
    in-flight requests (a held-open event stream stays in flight) and when the last one finished.
    A client that has been heard from recently is "connected"; that is the agent heartbeat."""

    def __init__(self, app) -> None:
        self.app = app
        self._lock = threading.Lock()
        self.inflight = 0
        self.last = -1e9

    async def __call__(self, scope, receive, send):
        if scope["type"] != "http":
            await self.app(scope, receive, send)
            return
        with self._lock:
            self.inflight += 1
            self.last = time.monotonic()
        try:
            await self.app(scope, receive, send)
        finally:
            with self._lock:
                self.inflight -= 1
                self.last = time.monotonic()

    def connected(self, now: float, grace_s: float) -> bool:
        with self._lock:
            return self.inflight > 0 or now - self.last <= grace_s


def build_app(mcp: MCPServer, token: str, host: str):
    """The ASGI app: token check outside, activity tracking inside. Returns (app, activity)."""
    inner = mcp.streamable_http_app(
        host=host,
        # Bearer auth already stops drive-by requests; DNS-rebinding checks would only get in the way
        # of reaching the Pi by IP.
        transport_security=TransportSecuritySettings(enable_dns_rebinding_protection=False),
    )
    activity = Activity(inner)
    return BearerAuth(activity, token), activity