luccapirovano/naowalk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

made the streams good even on an iffy network... audio latency just needs some work now

Browse Source
This commit is contained in:
Lucca Pirovano
2026-07-20 18:56:45 -04:00
parent 4a4625028b
commit 7d0b968b36
5 changed files with 312 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files
nao_video_server.py
+204 -93
View File
@@ -3,9 +3,33 @@
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 an MJPEG video stream over plain HTTP. Only
compressed JPEG bytes ever cross the phone tunnel, instead of raw RGB
frames.
the same box) and serves ONE persistent, length-framed TCP stream
carrying both compressed video (JPEG) and compressed audio (mu-law).
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 (all integers big-endian, via `struct`):
1 byte type 0x00 HELLO 0x01 VIDEO 0x02 AUDIO
4 bytes length
N bytes payload
HELLO payload: JSON {"rate": 16000, "channels": 1}
VIDEO payload: JPEG bytes
AUDIO payload: mu-law encoded 8-bit samples (mono)
Under load (slow/lossy tunnel), video quality and frame rate step
down automatically (see QUALITY_TIERS) and recover once sends are
fast again. Audio is prioritized over video each pass, since gaps in
audio are far more noticeable than a slightly stale picture.
Needs the SDK's own env vars, since the bindings aren't on a login
shell's default path:
@@ -14,18 +38,15 @@ shell's default path:
export LD_LIBRARY_PATH=/opt/aldebaran/lib
python2 nao_video_server.py
No Flask here on purpose - it's very unlikely to be installed on this
locked-down embedded OS and there's no easy way to pip install it
without network access on the robot. This uses only the stdlib
(BaseHTTPServer/SocketServer) plus qi and cv2, which we've confirmed
are already present.
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 time
import socket
import struct
import threading
import BaseHTTPServer
import SocketServer
import time
import cv2
import numpy as np
@@ -33,46 +54,108 @@ import qi
# ---- tunables ----
NAOQI_PORT = 9561 # local qi session port (confirmed working)
HTTP_PORT = 8000 # forward this through the phone tunnel
TCP_PORT = 8000 # forward this one port through the phone tunnel
CAMERA_RESOLUTION = 1 # 0=kQQVGA(160x120) 1=kQVGA(320x240) 2=kVGA(640x480)
CAMERA_COLORSPACE = 11 # kRGBColorSpace
CAMERA_FPS = 20
JPEG_QUALITY = 60 # 1-100 - tune this first if the link is still tight
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. Same pattern as naowalk.py's own _video_loop -
a stalled capture never blocks a client's HTTP request."""
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(
"MjpegServer", 0, CAMERA_RESOLUTION, CAMERA_COLORSPACE, CAMERA_FPS)
"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, JPEG_QUALITY])
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
@@ -85,80 +168,107 @@ class FrameGrabber(object):
pass
class Handler(BaseHTTPServer.BaseHTTPRequestHandler):
def log_message(self, fmt, *args):
pass # keep the robot's console quiet
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 do_GET(self):
if self.path == "/video":
self._serve_video()
elif self.path == "/battery":
self._serve_battery()
elif self.path == "/health":
self._ok()
else:
self.send_error(404)
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 do_POST(self):
length = int(self.headers.getheader("content-length", 0))
raw = self.rfile.read(length) if length else "{}"
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
try:
payload = json.loads(raw)
except ValueError:
payload = {}
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)
if self.path == "/move":
x = float(payload.get("x", 0.0))
y = float(payload.get("y", 0.0))
theta = float(payload.get("theta", 0.0))
self.server.motion.moveToward(x, y, theta)
self._ok()
elif self.path == "/stop":
self.server.motion.stopMove()
self._ok()
elif self.path == "/say":
self.server.tts.say(str(payload.get("text", "")))
self._ok()
else:
self.send_error(404)
def pop_all(self):
"""Drain everything queued right now, oldest first."""
with self._lock:
out = list(self._chunks)
self._chunks.clear()
return out
def _ok(self):
self.send_response(200)
self.send_header("Content-Type", "application/json")
self.end_headers()
self.wfile.write(b'{"ok": true}')
def _serve_battery(self):
def close(self):
try:
pct = self.server.battery.getBatteryCharge() if self.server.battery else -1
self.audio.unsubscribe(self._svc_name)
except Exception:
pct = -1
self.send_response(200)
self.send_header("Content-Type", "application/json")
self.end_headers()
self.wfile.write(json.dumps({"battery": pct}).encode())
def _serve_video(self):
self.send_response(200)
self.send_header("Content-Type", "multipart/x-mixed-replace; boundary=frame")
self.end_headers()
try:
while True:
jpeg = self.server.grabber.latest()
if jpeg is not None:
self.wfile.write(b"--frame\r\n")
self.wfile.write(b"Content-Type: image/jpeg\r\n")
self.wfile.write(b"Content-Length: %d\r\n\r\n" % len(jpeg))
self.wfile.write(jpeg)
self.wfile.write(b"\r\n")
time.sleep(1.0 / CAMERA_FPS)
except Exception:
pass # client disconnected - totally normal, not an error
pass
class ThreadingHTTPServer(SocketServer.ThreadingMixIn, BaseHTTPServer.HTTPServer):
daemon_threads = True
allow_reuse_address = True
class MediaServer(object):
"""Accepts one client at a time and interleaves video + audio onto
its socket. Audio is drained first every pass since gaps in it are
much more noticeable than the picture being a beat stale."""
def __init__(self, grabber, mic):
self.grabber = grabber
self.mic = mic
def serve_forever(self, host, port):
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("media 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("client connected: %s" % (addr,))
try:
self._serve_client(conn)
except Exception as e:
print("client disconnected (%s)" % e)
finally:
try:
conn.close()
except Exception:
pass
def _serve_client(self, conn):
self._send(conn, TYPE_HELLO,
json.dumps({"rate": AUDIO_RATE, "channels": AUDIO_CHANNELS}).encode("utf-8"))
last_jpeg = None
last_video_send = 0.0
while True:
did_something = False
if self.mic is not None:
for chunk in self.mic.pop_all():
self._send(conn, TYPE_AUDIO, chunk)
did_something = True
jpeg = self.grabber.latest()
_, min_interval = self.grabber.quality_and_interval()
now = time.time()
if jpeg is not None and jpeg is not last_jpeg and now - last_video_send >= min_interval:
t0 = time.time()
self._send(conn, TYPE_VIDEO, jpeg)
self.grabber.report_send_time(time.time() - t0)
last_jpeg = jpeg
last_video_send = now
did_something = True
if not did_something:
time.sleep(0.01)
def _send(self, conn, msg_type, payload):
conn.sendall(struct.pack(">BI", msg_type, len(payload)))
conn.sendall(payload)
def main():
@@ -166,22 +276,23 @@ def main():
session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT)
print("connected to local NAOqi on port %d" % NAOQI_PORT)
server = ThreadingHTTPServer(("0.0.0.0", HTTP_PORT), Handler)
server.grabber = FrameGrabber(session)
server.motion = session.service("ALMotion")
server.tts = session.service("ALTextToSpeech")
grabber = FrameGrabber(session)
mic = None
try:
server.battery = session.service("ALBattery")
except Exception:
server.battery = None
mic = MicGrabber(session)
print("mic capture ready (mu-law, %dHz)" % AUDIO_RATE)
except Exception as e:
print("mic capture unavailable (%s) - video only" % e)
print("serving on :%d (/video, /move, /stop, /say, /battery, /health)" % HTTP_PORT)
server = MediaServer(grabber, mic)
try:
server.serve_forever()
server.serve_forever("0.0.0.0", TCP_PORT)
except KeyboardInterrupt:
pass
finally:
server.grabber.close()
grabber.close()
if mic:
mic.close()
if __name__ == "__main__":