Initial commit
Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
This commit is contained in:
Claude Sonnet 5.5
@@ -0,0 +1,229 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user