230 lines
8.6 KiB
Python
230 lines
8.6 KiB
Python
#!/usr/bin/env python3
|
|
"""
|
|
Robot MCP server: runs on the Raspberry Pi and gives Hermes Agent the robot's eyes
|
|
(and, optionally, a neck).
|
|
|
|
Tools exposed
|
|
look() -> a JPEG still from the camera
|
|
turn_head(pan, tilt) -> aim the pan/tilt servos (only if ROBOT_SERVOS=1)
|
|
head_position() -> current pan/tilt (only if ROBOT_SERVOS=1)
|
|
|
|
Transport: MCP streamable HTTP at http://<pi>:<port>/mcp, guarded by a bearer token.
|
|
Audio is NOT handled here; it travels over PipeWire (see README).
|
|
|
|
Configuration is by environment variable (normally set in /etc/robot-mcp.env):
|
|
ROBOT_MCP_TOKEN required shared secret; Hermes sends "Authorization: Bearer <token>"
|
|
ROBOT_MCP_PORT 8765
|
|
ROBOT_MCP_BIND 0.0.0.0
|
|
ROBOT_CAM_SIZE 1280x720 still resolution handed to the model
|
|
ROBOT_CAM_QUALITY 85 JPEG quality
|
|
ROBOT_CAM_HFLIP 0 set 1 if the camera is mounted mirrored
|
|
ROBOT_CAM_VFLIP 0 set 1 (with HFLIP=1) if mounted upside down
|
|
ROBOT_SERVOS 0 set 1 to enable turn_head via a PCA9685 board
|
|
ROBOT_PAN_CHANNEL 0
|
|
ROBOT_TILT_CHANNEL 1
|
|
ROBOT_PAN_LIMIT 80 degrees either side of centre
|
|
ROBOT_TILT_LIMIT 45
|
|
ROBOT_PAN_INVERT 0 set 1 if "positive = left" comes out backwards
|
|
ROBOT_TILT_INVERT 0
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import hmac
|
|
import io
|
|
import logging
|
|
import os
|
|
import sys
|
|
import threading
|
|
import time
|
|
|
|
import uvicorn
|
|
from mcp.server.mcpserver import Image, MCPServer
|
|
from mcp.server.transport_security import TransportSecuritySettings
|
|
|
|
log = logging.getLogger("robot-mcp")
|
|
logging.basicConfig(level=logging.INFO, format="%(asctime)s %(levelname)s %(name)s: %(message)s")
|
|
|
|
|
|
def env_flag(name: str, default: str = "0") -> bool:
|
|
return os.environ.get(name, default).strip().lower() in ("1", "true", "yes", "on")
|
|
|
|
|
|
def env_int(name: str, default: int) -> int:
|
|
return int(os.environ.get(name, str(default)))
|
|
|
|
|
|
TOKEN = os.environ.get("ROBOT_MCP_TOKEN", "").strip()
|
|
PORT = env_int("ROBOT_MCP_PORT", 8765)
|
|
BIND = os.environ.get("ROBOT_MCP_BIND", "0.0.0.0")
|
|
CAM_W, CAM_H = (int(v) for v in os.environ.get("ROBOT_CAM_SIZE", "1280x720").lower().split("x"))
|
|
CAM_QUALITY = env_int("ROBOT_CAM_QUALITY", 85)
|
|
|
|
|
|
# --------------------------------------------------------------------------- camera
|
|
|
|
class Camera:
|
|
"""Keeps Picamera2 running so a still is ready in well under a second."""
|
|
|
|
def __init__(self) -> None:
|
|
from libcamera import Transform # type: ignore[import-not-found]
|
|
from picamera2 import Picamera2 # type: ignore[import-not-found]
|
|
|
|
self._lock = threading.Lock()
|
|
self.cam = Picamera2()
|
|
config = self.cam.create_still_configuration(
|
|
main={"size": (CAM_W, CAM_H)},
|
|
transform=Transform(hflip=env_flag("ROBOT_CAM_HFLIP"), vflip=env_flag("ROBOT_CAM_VFLIP")),
|
|
buffer_count=2,
|
|
)
|
|
self.cam.configure(config)
|
|
self.cam.options["quality"] = CAM_QUALITY
|
|
self.has_autofocus = "AfMode" in self.cam.camera_controls
|
|
self.cam.start()
|
|
time.sleep(1.0) # let auto-exposure settle once at startup
|
|
log.info("camera ready at %dx%d (autofocus: %s)", CAM_W, CAM_H, self.has_autofocus)
|
|
|
|
def snapshot(self) -> bytes:
|
|
with self._lock:
|
|
if self.has_autofocus:
|
|
# Camera Module 3: run one focus sweep so whatever is in front is sharp.
|
|
self.cam.autofocus_cycle()
|
|
buf = io.BytesIO()
|
|
self.cam.capture_file(buf, format="jpeg")
|
|
return buf.getvalue()
|
|
|
|
|
|
# --------------------------------------------------------------------------- servos
|
|
|
|
class Head:
|
|
"""Pan/tilt on a PCA9685 servo board. Angles are degrees from centre."""
|
|
|
|
def __init__(self) -> None:
|
|
from adafruit_servokit import ServoKit # type: ignore[import-not-found]
|
|
|
|
self._lock = threading.Lock()
|
|
self.kit = ServoKit(channels=16)
|
|
self.pan_ch = env_int("ROBOT_PAN_CHANNEL", 0)
|
|
self.tilt_ch = env_int("ROBOT_TILT_CHANNEL", 1)
|
|
self.pan_limit = env_int("ROBOT_PAN_LIMIT", 80)
|
|
self.tilt_limit = env_int("ROBOT_TILT_LIMIT", 45)
|
|
self.pan_sign = -1 if env_flag("ROBOT_PAN_INVERT") else 1
|
|
self.tilt_sign = -1 if env_flag("ROBOT_TILT_INVERT") else 1
|
|
self.pan = 0.0
|
|
self.tilt = 0.0
|
|
self.move(0.0, 0.0)
|
|
log.info("servos ready (pan ch %d, tilt ch %d)", self.pan_ch, self.tilt_ch)
|
|
|
|
def move(self, pan: float, tilt: float) -> tuple[float, float]:
|
|
pan = max(-self.pan_limit, min(self.pan_limit, pan))
|
|
tilt = max(-self.tilt_limit, min(self.tilt_limit, tilt))
|
|
with self._lock:
|
|
self.kit.servo[self.pan_ch].angle = 90 + self.pan_sign * pan
|
|
self.kit.servo[self.tilt_ch].angle = 90 + self.tilt_sign * tilt
|
|
self.pan, self.tilt = pan, tilt
|
|
time.sleep(0.4) # let the servos arrive and stop jittering before any photo
|
|
return pan, tilt
|
|
|
|
|
|
# --------------------------------------------------------------------------- MCP server
|
|
|
|
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."
|
|
),
|
|
)
|
|
|
|
camera: Camera | None = None
|
|
head: Head | None = None
|
|
|
|
|
|
@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."""
|
|
if camera is None:
|
|
raise RuntimeError("camera is not available on the robot")
|
|
t0 = time.monotonic()
|
|
data = camera.snapshot()
|
|
log.info("look(): %d bytes in %.2fs", len(data), time.monotonic() - t0)
|
|
return Image(data=data, format="jpeg")
|
|
|
|
|
|
def register_head_tools() -> 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."""
|
|
assert head is not None
|
|
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."""
|
|
assert head is not None
|
|
return f"pan {head.pan:+.0f} degrees, tilt {head.tilt:+.0f} degrees"
|
|
|
|
|
|
# --------------------------------------------------------------------------- auth wrapper
|
|
|
|
class BearerAuth:
|
|
"""Minimal 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)
|
|
|
|
|
|
def build_app():
|
|
app = mcp.streamable_http_app(
|
|
host=BIND,
|
|
# Bearer auth already stops drive-by requests; DNS-rebinding checks would only
|
|
# get in the way of reaching the Pi by IP or by robot.local.
|
|
transport_security=TransportSecuritySettings(enable_dns_rebinding_protection=False),
|
|
)
|
|
return BearerAuth(app, TOKEN)
|
|
|
|
|
|
def main() -> None:
|
|
global camera, head
|
|
|
|
if len(TOKEN) < 16:
|
|
sys.exit("ROBOT_MCP_TOKEN is missing or too short (generate one with: openssl rand -hex 32)")
|
|
|
|
try:
|
|
camera = Camera()
|
|
except Exception as exc: # keep serving servos even if the camera ribbon is loose
|
|
log.error("camera failed to start: %s", exc)
|
|
|
|
if env_flag("ROBOT_SERVOS"):
|
|
try:
|
|
head = Head()
|
|
register_head_tools()
|
|
except Exception as exc:
|
|
log.error("servos failed to start, turn_head disabled: %s", exc)
|
|
|
|
log.info("serving MCP on http://%s:%d/mcp", BIND, PORT)
|
|
uvicorn.run(build_app(), host=BIND, port=PORT, log_level="warning")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|