#!/usr/bin/env python3
"""Robot finto: implementa l'API §6 della SPEC per provare il Nido senza hardware.
python fake_robot.py --port 8781 --stream-port 8782 [--token segreto] [--movimento]
- GET / pagina minima
- GET /capture JPEG generato (vista "finta" che cambia con i movimenti)
- GET /stream MJPEG sulla porta stream (10 fps)
- GET /api/status telemetria JSON
- GET/POST /api/gait, /api/calib
- WS /ws comandi + telemetria a 5 Hz, watchdog di movimento 1 s
- WS /audio invia PCM 16 kHz (tono tenue) e conta i byte ricevuti per lo speaker
- GET /debug/log (solo simulatore) comandi ricevuti, byte audio, connessioni stream
"""
from __future__ import annotations
import argparse
import asyncio
import io
import json
import math
import time
from aiohttp import WSMsgType, web
from PIL import Image, ImageDraw # Pillow è nei requirements
class FintoRobot:
def __init__(self, token: str = "", movimento: bool = False):
self.token = token
self.movimento = movimento
self.pose, self.mode, self.speed = "stand", "cpg", 1.0
self.vx = self.wz = 0.0
self.x = self.y = self.th = 0.0
self.ultimo_move = 0.0
self.eyes = (1, 1)
self.log: list[dict] = []
self.clients: set[web.WebSocketResponse] = set()
self.audio_rx = 0
self.stream_conn = 0
self.stream_attivi = 0
self.gait = {"version": 1, "type": "cpg", "name": "finto", "freq": 1.6,
"joints": {"hip_roll": {"amp": 0.18, "phase": 0.0, "offset": 0.02},
"hip_pitch": {"amp": 0.3, "phase": 1.57, "offset": 0.0},
"ankle_pitch": {"amp": 0.2, "phase": 1.57, "offset": 0.0}},
"feedback": {"roll_gain": 0.0, "pitch_gain": 0.0}}
self.calib = [0, 0, 0, 0, 0, 0]
self.t0 = time.monotonic()
# --- simulazione ------------------------------------------------------------------
@property
def moving(self) -> bool:
return (self.vx or self.wz) and time.monotonic() - self.ultimo_move < 1.0
def passo(self, dt: float) -> None:
if (self.vx or self.wz) and time.monotonic() - self.ultimo_move >= 1.0:
self.vx = self.wz = 0.0 # watchdog 1 s
self.th += self.wz * 0.8 * dt * self.speed
self.x += math.cos(self.th) * self.vx * 0.1 * dt * self.speed
self.y += math.sin(self.th) * self.vx * 0.1 * dt * self.speed
def tel(self) -> dict:
t = time.monotonic() - self.t0
return {"t": "tel", "roll": round(0.01 * math.sin(t * 3) * (1 if self.moving else 0.1), 3),
"pitch": round(-0.02 + 0.01 * math.cos(t * 3), 3), "vbat": round(8.2 - (t / 3600) * 0.3, 2),
"mode": self.mode, "pose": self.pose, "moving": bool(self.moving), "rssi": -55,
"clients": len(self.clients), "speed": self.speed, "dist": 42}
def jpeg(self) -> bytes:
w, h = 320, 240
im = Image.new("RGB", (w, h), (40, 46, 58))
d = ImageDraw.Draw(im)
# "stanza" che ruota con la direzione del robot
for i in range(12):
a = i * math.pi / 6 - self.th
px = int(w / 2 + math.sin(a) * 260)
col = (70 + 15 * (i % 4), 90 + 10 * (i % 3), 120)
if -40 < px < w + 40 and math.cos(a) > 0:
d.rectangle([px - 18, 60, px + 18, 170], fill=col)
d.rectangle([0, 170, w, h], fill=(60, 60, 66))
if self.movimento and int(time.monotonic() - self.t0) % 20 < 6:
cx = int(60 + (time.monotonic() * 50) % 200)
d.ellipse([cx - 25, 70, cx + 25, 120], fill=(240, 200, 150))
d.rectangle([cx - 30, 120, cx + 30, 200], fill=(200, 60, 60))
d.text((6, 6), f"PULCINO FINTO x={self.x:.2f} y={self.y:.2f} th={math.degrees(self.th):.0f}", fill=(230, 230, 230))
d.text((6, 20), f"posa={self.pose} {'in movimento' if self.moving else 'fermo'} {time.strftime('%H:%M:%S')}",
fill=(180, 220, 255))
b = io.BytesIO()
im.save(b, "JPEG", quality=70)
return b.getvalue()
def ok_token(self, request: web.Request) -> bool:
return not self.token or request.query.get("token") == self.token
def applica(self, c: dict) -> None:
self.log.append({"t": time.time(), **c})
del self.log[:-500]
k = c.get("cmd")
if k == "move":
self.vx, self.wz = float(c.get("vx", 0)), float(c.get("wz", 0))
self.ultimo_move = time.monotonic()
elif k == "stop":
self.vx = self.wz = 0.0
elif k == "pose":
self.pose = c.get("name", "stand")
elif k == "mode":
self.mode = c.get("m", "cpg")
elif k == "speed":
self.speed = float(c.get("v", 1))
elif k == "eyes":
self.eyes = (c.get("l", 1), c.get("r", 1))
# --- handler ------------------------------------------------------------------------
async def index(self, r):
return web.Response(text="Pulcino finto
", content_type="text/html")
async def capture(self, r):
return web.Response(body=self.jpeg(), content_type="image/jpeg")
async def status(self, r):
return web.json_response({**self.tel(), "x": self.x, "y": self.y, "th": self.th, "eyes": self.eyes})
async def gait_h(self, r):
if r.method == "POST":
if not self.ok_token(r):
raise web.HTTPUnauthorized()
self.gait = await r.json()
return web.json_response(self.gait)
async def calib_h(self, r):
if r.method == "POST":
if not self.ok_token(r):
raise web.HTTPUnauthorized()
self.calib = (await r.json())[:6]
return web.json_response(self.calib)
async def debug(self, r):
return web.json_response({"log": self.log[-200:], "audio_rx": self.audio_rx,
"stream_conn": self.stream_conn, "stream_attivi": self.stream_attivi,
"ws_clients": len(self.clients)})
async def ws(self, r):
ws = web.WebSocketResponse(heartbeat=20)
await ws.prepare(r)
self.clients.add(ws)
autorizzato = self.ok_token(r)
try:
async for m in ws:
if m.type == WSMsgType.TEXT:
try:
c = json.loads(m.data)
except ValueError:
continue
if c.get("cmd") == "hello":
self.log.append({"t": time.time(), **c})
continue
if autorizzato:
self.applica(c)
finally:
self.clients.discard(ws)
return ws
async def audio(self, r):
ws = web.WebSocketResponse()
await ws.prepare(r)
async def mic():
n = 0
while not ws.closed:
# 64 ms di tono 440 Hz tenue
buf = bytearray()
for i in range(1024):
v = int(1500 * math.sin(2 * math.pi * 440 * (n + i) / 16000))
buf += v.to_bytes(2, "little", signed=True)
n += 1024
try:
await ws.send_bytes(bytes(buf))
except Exception:
return
await asyncio.sleep(0.064)
t = asyncio.create_task(mic())
try:
async for m in ws:
if m.type == WSMsgType.BINARY:
self.audio_rx += len(m.data)
finally:
t.cancel()
return ws
async def stream(self, r):
resp = web.StreamResponse(headers={"Content-Type": "multipart/x-mixed-replace; boundary=123456789000000000000987654321"})
await resp.prepare(r)
self.stream_conn += 1
self.stream_attivi += 1
try:
while True:
j = self.jpeg()
await resp.write(b"--123456789000000000000987654321\r\nContent-Type: image/jpeg\r\n"
+ f"Content-Length: {len(j)}\r\n\r\n".encode() + j + b"\r\n")
await asyncio.sleep(0.1)
except (ConnectionResetError, asyncio.CancelledError):
pass
finally:
self.stream_attivi -= 1
return resp
async def telemetria(self):
ultimo = time.monotonic()
while True:
await asyncio.sleep(0.2)
now = time.monotonic()
self.passo(now - ultimo)
ultimo = now
if self.clients:
s = json.dumps(self.tel())
for ws in list(self.clients):
try:
await ws.send_str(s)
except Exception:
pass
def crea_app_robot(robot: FintoRobot) -> tuple[web.Application, web.Application]:
app = web.Application()
app.router.add_get("/", robot.index)
app.router.add_get("/capture", robot.capture)
app.router.add_get("/api/status", robot.status)
app.router.add_route("*", "/api/gait", robot.gait_h)
app.router.add_route("*", "/api/calib", robot.calib_h)
app.router.add_get("/ws", robot.ws)
app.router.add_get("/audio", robot.audio)
app.router.add_get("/debug/log", robot.debug)
tel_task: list[asyncio.Task] = []
async def avvio(_):
tel_task.append(asyncio.create_task(robot.telemetria()))
async def fine(_):
for t in tel_task:
t.cancel()
for ws in list(robot.clients):
await ws.close()
app.on_startup.append(avvio)
app.on_shutdown.append(fine)
sapp = web.Application()
sapp.router.add_get("/stream", robot.stream)
sapp.router.add_get("/capture", robot.capture) # come il firmware: /capture anche su :81
return app, sapp
async def avvia(host: str, port: int, stream_port: int, robot: FintoRobot) -> list[web.AppRunner]:
app, sapp = crea_app_robot(robot)
runners = []
for a, p in ((app, port), (sapp, stream_port)):
rn = web.AppRunner(a, access_log=None, shutdown_timeout=1)
await rn.setup()
await web.TCPSite(rn, host, p).start()
runners.append(rn)
return runners
def main() -> None:
ap = argparse.ArgumentParser(description="Robot Pulcino finto per test")
ap.add_argument("--host", default="127.0.0.1")
ap.add_argument("--port", type=int, default=8781)
ap.add_argument("--stream-port", type=int, default=8782)
ap.add_argument("--token", default="")
ap.add_argument("--movimento", action="store_true", help="fa comparire ogni 20 s una 'persona' nell'immagine")
a = ap.parse_args()
async def run():
runners = await avvia(a.host, a.port, a.stream_port, FintoRobot(a.token, a.movimento))
print(f"Pulcino finto: http://{a.host}:{a.port} stream http://{a.host}:{a.stream_port}/stream", flush=True)
try:
await asyncio.Event().wait()
finally:
for r in runners:
await r.cleanup()
try:
asyncio.run(run())
except KeyboardInterrupt:
pass
if __name__ == "__main__":
main()