# -*- coding: utf-8 -*- """ nao_video_server.py - RUNS ON THE ROBOT, under its own Python 2.7. Opens a qi session to NAOqi over localhost (zero network hop - it's the same box) and serves TWO persistent, length-framed TCP streams - one for compressed video (JPEG), one for compressed audio (mu-law) - each on its own port/connection. They used to share one connection, but a single big video frame mid-send could block audio behind it for its whole transfer time, which is a bad tradeoff for something as latency-sensitive as audio. Splitting them means a slow video frame can never delay audio. Why not HTTP: the phone tunnel only forwards specific TCP ports, so this still rides over TCP - but a 5-byte binary header has none of HTTP's chunked/multipart-boundary parsing overhead, and it's trivial to keep alive and reconnect on a flaky link. Why mic capture lives here now: it used to be shipped raw (16-bit PCM, uncompressed) straight through NAOqi's own pub/sub over the qi RPC session the client holds - i.e. across the tunnel, uncompressed, the whole time. Subscribing to ALAudioDevice locally (same zero-hop trick as the camera) means only mu-law-compressed bytes ever cross the tunnel, half the size of raw PCM. Wire format, identical on both connections (all integers big-endian, via `struct`): 1 byte type 0x00 HELLO 0x01 VIDEO 0x02 AUDIO 4 bytes length N bytes payload HELLO payload: JSON, informational only VIDEO payload: JPEG bytes (video connection only) AUDIO payload: mu-law encoded 8-bit samples, mono (audio connection only) Under load (slow/lossy tunnel), video quality and frame rate step down automatically (see QUALITY_TIERS) and recover once sends are fast again - and since encoding is throttled to match, this also cuts CPU use on a congested link instead of just wasting it on frames that would've been discarded anyway. Needs the SDK's own env vars, since the bindings aren't on a login shell's default path: export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages export LD_LIBRARY_PATH=/opt/aldebaran/lib python2 nao_video_server.py Stdlib only (BaseHTTPServer/Flask are gone entirely now) plus qi, cv2, and numpy, which are already confirmed present on the robot. """ import collections import json import socket import struct import threading import time import cv2 import numpy as np import qi # ---- tunables ---- NAOQI_PORT = 9561 # local qi session port (confirmed working) VIDEO_PORT = 8000 # forward both of these through the phone tunnel AUDIO_PORT = 8001 # kept on its own connection so a slow video # frame can never block/delay audio delivery CAMERA_RESOLUTION = 1 # 0=kQQVGA(160x120) 1=kQVGA(320x240) 2=kVGA(640x480) CAMERA_COLORSPACE = 11 # kRGBColorSpace CAMERA_FPS = 20 AUDIO_RATE = 16000 AUDIO_CHANNELS = 1 MIC_CHANNEL = 1 # 1=left 2=right 3=front 4=rear (ALAudioDevice channel index) AUDIO_QUEUE_MAX = 60 # chunks buffered before we start dropping the oldest # (jpeg_quality, min_seconds_between_frames) - best link first, worst last. # The server steps down a tier when a video send is slow, and back up # after a run of fast ones. QUALITY_TIERS = [ (70, 0.05), (55, 0.07), (40, 0.10), (25, 0.15), ] SLOW_SEND_THRESHOLD = 0.20 # seconds - a send this slow means congestion FAST_STREAK_TO_RECOVER = 30 # consecutive quick sends before stepping back up TYPE_HELLO = 0x00 TYPE_VIDEO = 0x01 TYPE_AUDIO = 0x02 def encode_ulaw(pcm16): """int16 numpy array -> uint8 numpy array, mu-law companded. Must match decode_ulaw() in naowalk.py exactly, or the far end just hears noise - keep the two in sync if you touch this.""" mu = 255.0 x = np.clip(pcm16.astype(np.float64) / 32768.0, -1.0, 1.0) y = np.sign(x) * np.log1p(mu * np.abs(x)) / np.log1p(mu) return (((y + 1.0) / 2.0) * 255.0).round().astype(np.uint8) class FrameGrabber(object): """Background thread: keeps the single most recent JPEG-encoded frame available, at whatever quality tier the link currently supports. A stalled capture never blocks a client's read.""" def __init__(self, session): self.video = session.service("ALVideoDevice") self.client = self.video.subscribeCamera( "MediaServer", 0, CAMERA_RESOLUTION, CAMERA_COLORSPACE, CAMERA_FPS) self._lock = threading.Lock() self._jpeg = None self._tier = 0 self._fast_streak = 0 self._stop = threading.Event() self._thread = threading.Thread(target=self._loop) self._thread.daemon = True self._thread.start() def _loop(self): last_encode = 0.0 while not self._stop.is_set(): try: image = self.video.getImageRemote(self.client) if not image or len(image) < 7: continue # getImageRemote() blocks until the next camera frame is # ready (~1/CAMERA_FPS), so this loop is already paced - # but under a congested tier we send far less often than # that, so skip the (expensive) convert+encode work for # frames we're just going to overwrite unsent anyway. quality, min_interval = self.quality_and_interval() now = time.time() if now - last_encode < min_interval: continue w, h = image[0], image[1] arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3)) bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) ok, buf = cv2.imencode(".jpg", bgr, [cv2.IMWRITE_JPEG_QUALITY, quality]) if ok: with self._lock: self._jpeg = buf.tobytes() last_encode = now except Exception as e: print("frame grab error: %s" % e) time.sleep(0.2) def quality_and_interval(self): with self._lock: return QUALITY_TIERS[self._tier] def report_send_time(self, elapsed): """Called by the writer thread after each video send so we can adapt to how the link is actually behaving right now.""" with self._lock: if elapsed > SLOW_SEND_THRESHOLD: self._fast_streak = 0 if self._tier < len(QUALITY_TIERS) - 1: self._tier += 1 print("link looks congested - dropping to quality tier %d" % self._tier) else: self._fast_streak += 1 if self._fast_streak >= FAST_STREAK_TO_RECOVER and self._tier > 0: self._tier -= 1 self._fast_streak = 0 print("link recovered - raising to quality tier %d" % self._tier) def latest(self): with self._lock: return self._jpeg def close(self): self._stop.set() try: self.video.unsubscribe(self.client) except Exception: pass class MicGrabber(object): """Subscribes to the robot's own mic locally (zero-hop, same trick as FrameGrabber) and mu-law-encodes each chunk as it arrives. This replaces the old design where raw PCM was pushed to the client directly through NAOqi's pub/sub over the (tunneled) qi session.""" def __init__(self, session, channel=MIC_CHANNEL): self.audio = session.service("ALAudioDevice") self._svc_name = "MicRelay" self._lock = threading.Lock() self._chunks = collections.deque(maxlen=AUDIO_QUEUE_MAX) session.registerService(self._svc_name, self) time.sleep(0.5) # let NAOqi's service directory propagate the # registration before ALAudioDevice looks it up # by name, or subscribe() fails to find it self.audio.setClientPreferences(self._svc_name, AUDIO_RATE, channel, 0) self.audio.subscribe(self._svc_name) def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): try: pcm = np.frombuffer(bytes(buffer), dtype=np.int16) chunk = encode_ulaw(pcm).tobytes() with self._lock: self._chunks.append(chunk) except Exception as e: print("mic encode error: %s" % e) def pop_all(self): """Drain everything queued right now, oldest first.""" with self._lock: out = list(self._chunks) self._chunks.clear() return out def close(self): try: self.audio.unsubscribe(self._svc_name) except Exception: pass def _send(conn, msg_type, payload): conn.sendall(struct.pack(">BI", msg_type, len(payload))) conn.sendall(payload) def serve_video(grabber, host, port): """Video gets its own connection/thread - a slow sendall() here (a big JPEG frame over a bad link) must never be able to delay audio, which used to share this same socket and paid for it in latency.""" sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) sock.bind((host, port)) sock.listen(1) print("video server listening on :%d" % port) while True: conn, addr = sock.accept() conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) conn.settimeout(5.0) print("video client connected: %s" % (addr,)) last_jpeg = None last_send = 0.0 try: _send(conn, TYPE_HELLO, json.dumps({"stream": "video"}).encode("utf-8")) while True: jpeg = grabber.latest() _, min_interval = grabber.quality_and_interval() now = time.time() if jpeg is not None and jpeg is not last_jpeg and now - last_send >= min_interval: t0 = time.time() _send(conn, TYPE_VIDEO, jpeg) grabber.report_send_time(time.time() - t0) last_jpeg = jpeg last_send = now else: time.sleep(0.01) except Exception as e: print("video client disconnected (%s)" % e) finally: try: conn.close() except Exception: pass def serve_audio(mic, host, port): """Audio's own connection/thread. Checked eagerly (short sleep, not paced like video) since gaps are far more noticeable than a video frame being a beat stale.""" sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) sock.bind((host, port)) sock.listen(1) print("audio server listening on :%d" % port) while True: conn, addr = sock.accept() conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) conn.settimeout(5.0) print("audio client connected: %s" % (addr,)) try: _send(conn, TYPE_HELLO, json.dumps({"rate": AUDIO_RATE, "channels": AUDIO_CHANNELS}).encode("utf-8")) while True: chunks = mic.pop_all() if mic is not None else [] if chunks: for chunk in chunks: _send(conn, TYPE_AUDIO, chunk) else: time.sleep(0.005) except Exception as e: print("audio client disconnected (%s)" % e) finally: try: conn.close() except Exception: pass def main(): session = qi.Session() session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT) print("connected to local NAOqi on port %d" % NAOQI_PORT) grabber = FrameGrabber(session) mic = None try: mic = MicGrabber(session) print("mic capture ready (mu-law, %dHz)" % AUDIO_RATE) except Exception as e: print("mic capture unavailable (%s) - video only" % e) video_thread = threading.Thread(target=serve_video, args=(grabber, "0.0.0.0", VIDEO_PORT)) video_thread.daemon = True video_thread.start() try: serve_audio(mic, "0.0.0.0", AUDIO_PORT) except KeyboardInterrupt: pass finally: grabber.close() if mic: mic.close() if __name__ == "__main__": main()