made the streams good even on an iffy network... audio latency just needs some work now
This commit is contained in:
@@ -0,0 +1,4 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
||||||
|
#python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
||||||
|
python3.11 naowalk.py --ip 127.0.0.1 --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 2222 --media-relay 127.0.0.1:8000
|
||||||
Binary file not shown.
+204
-93
@@ -3,9 +3,33 @@
|
|||||||
nao_video_server.py - RUNS ON THE ROBOT, under its own Python 2.7.
|
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
|
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
|
the same box) and serves ONE persistent, length-framed TCP stream
|
||||||
compressed JPEG bytes ever cross the phone tunnel, instead of raw RGB
|
carrying both compressed video (JPEG) and compressed audio (mu-law).
|
||||||
frames.
|
|
||||||
|
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
|
Needs the SDK's own env vars, since the bindings aren't on a login
|
||||||
shell's default path:
|
shell's default path:
|
||||||
@@ -14,18 +38,15 @@ shell's default path:
|
|||||||
export LD_LIBRARY_PATH=/opt/aldebaran/lib
|
export LD_LIBRARY_PATH=/opt/aldebaran/lib
|
||||||
python2 nao_video_server.py
|
python2 nao_video_server.py
|
||||||
|
|
||||||
No Flask here on purpose - it's very unlikely to be installed on this
|
Stdlib only (BaseHTTPServer/Flask are gone entirely now) plus qi,
|
||||||
locked-down embedded OS and there's no easy way to pip install it
|
cv2, and numpy, which are already confirmed present on the robot.
|
||||||
without network access on the robot. This uses only the stdlib
|
|
||||||
(BaseHTTPServer/SocketServer) plus qi and cv2, which we've confirmed
|
|
||||||
are already present.
|
|
||||||
"""
|
"""
|
||||||
|
import collections
|
||||||
import json
|
import json
|
||||||
import time
|
import socket
|
||||||
|
import struct
|
||||||
import threading
|
import threading
|
||||||
|
import time
|
||||||
import BaseHTTPServer
|
|
||||||
import SocketServer
|
|
||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -33,46 +54,108 @@ import qi
|
|||||||
|
|
||||||
# ---- tunables ----
|
# ---- tunables ----
|
||||||
NAOQI_PORT = 9561 # local qi session port (confirmed working)
|
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_RESOLUTION = 1 # 0=kQQVGA(160x120) 1=kQVGA(320x240) 2=kVGA(640x480)
|
||||||
CAMERA_COLORSPACE = 11 # kRGBColorSpace
|
CAMERA_COLORSPACE = 11 # kRGBColorSpace
|
||||||
CAMERA_FPS = 20
|
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):
|
class FrameGrabber(object):
|
||||||
"""Background thread: keeps the single most recent JPEG-encoded
|
"""Background thread: keeps the single most recent JPEG-encoded
|
||||||
frame available. Same pattern as naowalk.py's own _video_loop -
|
frame available, at whatever quality tier the link currently
|
||||||
a stalled capture never blocks a client's HTTP request."""
|
supports. A stalled capture never blocks a client's read."""
|
||||||
|
|
||||||
def __init__(self, session):
|
def __init__(self, session):
|
||||||
self.video = session.service("ALVideoDevice")
|
self.video = session.service("ALVideoDevice")
|
||||||
self.client = self.video.subscribeCamera(
|
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._lock = threading.Lock()
|
||||||
self._jpeg = None
|
self._jpeg = None
|
||||||
|
self._tier = 0
|
||||||
|
self._fast_streak = 0
|
||||||
self._stop = threading.Event()
|
self._stop = threading.Event()
|
||||||
self._thread = threading.Thread(target=self._loop)
|
self._thread = threading.Thread(target=self._loop)
|
||||||
self._thread.daemon = True
|
self._thread.daemon = True
|
||||||
self._thread.start()
|
self._thread.start()
|
||||||
|
|
||||||
def _loop(self):
|
def _loop(self):
|
||||||
|
last_encode = 0.0
|
||||||
while not self._stop.is_set():
|
while not self._stop.is_set():
|
||||||
try:
|
try:
|
||||||
image = self.video.getImageRemote(self.client)
|
image = self.video.getImageRemote(self.client)
|
||||||
if not image or len(image) < 7:
|
if not image or len(image) < 7:
|
||||||
continue
|
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]
|
w, h = image[0], image[1]
|
||||||
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
|
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
|
||||||
bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
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:
|
if ok:
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._jpeg = buf.tobytes()
|
self._jpeg = buf.tobytes()
|
||||||
|
last_encode = now
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print("frame grab error: %s" % e)
|
print("frame grab error: %s" % e)
|
||||||
time.sleep(0.2)
|
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):
|
def latest(self):
|
||||||
with self._lock:
|
with self._lock:
|
||||||
return self._jpeg
|
return self._jpeg
|
||||||
@@ -85,80 +168,107 @@ class FrameGrabber(object):
|
|||||||
pass
|
pass
|
||||||
|
|
||||||
|
|
||||||
class Handler(BaseHTTPServer.BaseHTTPRequestHandler):
|
class MicGrabber(object):
|
||||||
def log_message(self, fmt, *args):
|
"""Subscribes to the robot's own mic locally (zero-hop, same trick
|
||||||
pass # keep the robot's console quiet
|
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):
|
def __init__(self, session, channel=MIC_CHANNEL):
|
||||||
if self.path == "/video":
|
self.audio = session.service("ALAudioDevice")
|
||||||
self._serve_video()
|
self._svc_name = "MicRelay"
|
||||||
elif self.path == "/battery":
|
self._lock = threading.Lock()
|
||||||
self._serve_battery()
|
self._chunks = collections.deque(maxlen=AUDIO_QUEUE_MAX)
|
||||||
elif self.path == "/health":
|
session.registerService(self._svc_name, self)
|
||||||
self._ok()
|
time.sleep(0.5) # let NAOqi's service directory propagate the
|
||||||
else:
|
# registration before ALAudioDevice looks it up
|
||||||
self.send_error(404)
|
# 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):
|
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
||||||
length = int(self.headers.getheader("content-length", 0))
|
|
||||||
raw = self.rfile.read(length) if length else "{}"
|
|
||||||
try:
|
try:
|
||||||
payload = json.loads(raw)
|
pcm = np.frombuffer(bytes(buffer), dtype=np.int16)
|
||||||
except ValueError:
|
chunk = encode_ulaw(pcm).tobytes()
|
||||||
payload = {}
|
with self._lock:
|
||||||
|
self._chunks.append(chunk)
|
||||||
|
except Exception as e:
|
||||||
|
print("mic encode error: %s" % e)
|
||||||
|
|
||||||
if self.path == "/move":
|
def pop_all(self):
|
||||||
x = float(payload.get("x", 0.0))
|
"""Drain everything queued right now, oldest first."""
|
||||||
y = float(payload.get("y", 0.0))
|
with self._lock:
|
||||||
theta = float(payload.get("theta", 0.0))
|
out = list(self._chunks)
|
||||||
self.server.motion.moveToward(x, y, theta)
|
self._chunks.clear()
|
||||||
self._ok()
|
return out
|
||||||
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 _ok(self):
|
def close(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):
|
|
||||||
try:
|
try:
|
||||||
pct = self.server.battery.getBatteryCharge() if self.server.battery else -1
|
self.audio.unsubscribe(self._svc_name)
|
||||||
except Exception:
|
except Exception:
|
||||||
pct = -1
|
pass
|
||||||
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
|
|
||||||
|
|
||||||
|
|
||||||
class ThreadingHTTPServer(SocketServer.ThreadingMixIn, BaseHTTPServer.HTTPServer):
|
class MediaServer(object):
|
||||||
daemon_threads = True
|
"""Accepts one client at a time and interleaves video + audio onto
|
||||||
allow_reuse_address = True
|
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():
|
def main():
|
||||||
@@ -166,22 +276,23 @@ def main():
|
|||||||
session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT)
|
session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT)
|
||||||
print("connected to local NAOqi on port %d" % NAOQI_PORT)
|
print("connected to local NAOqi on port %d" % NAOQI_PORT)
|
||||||
|
|
||||||
server = ThreadingHTTPServer(("0.0.0.0", HTTP_PORT), Handler)
|
grabber = FrameGrabber(session)
|
||||||
server.grabber = FrameGrabber(session)
|
mic = None
|
||||||
server.motion = session.service("ALMotion")
|
|
||||||
server.tts = session.service("ALTextToSpeech")
|
|
||||||
try:
|
try:
|
||||||
server.battery = session.service("ALBattery")
|
mic = MicGrabber(session)
|
||||||
except Exception:
|
print("mic capture ready (mu-law, %dHz)" % AUDIO_RATE)
|
||||||
server.battery = None
|
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:
|
try:
|
||||||
server.serve_forever()
|
server.serve_forever("0.0.0.0", TCP_PORT)
|
||||||
except KeyboardInterrupt:
|
except KeyboardInterrupt:
|
||||||
pass
|
pass
|
||||||
finally:
|
finally:
|
||||||
server.grabber.close()
|
grabber.close()
|
||||||
|
if mic:
|
||||||
|
mic.close()
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|||||||
+103
-60
@@ -13,7 +13,7 @@ import pygame
|
|||||||
import subprocess
|
import subprocess
|
||||||
import os
|
import os
|
||||||
import json
|
import json
|
||||||
import re
|
import struct
|
||||||
import socket
|
import socket
|
||||||
|
|
||||||
# Import the new music module
|
# Import the new music module
|
||||||
@@ -33,12 +33,37 @@ try:
|
|||||||
except ImportError:
|
except ImportError:
|
||||||
HAS_SSH = False
|
HAS_SSH = False
|
||||||
|
|
||||||
# --- HTTP client for the on-robot MJPEG relay (see nao_video_server.py) ---
|
# ==========================================
|
||||||
try:
|
# MEDIA RELAY WIRE PROTOCOL (see nao_video_server.py)
|
||||||
import requests
|
# One TCP connection, length-framed: [1 byte type][4 byte length][payload].
|
||||||
HAS_REQUESTS = True
|
# Replaces the old HTTP MJPEG pull - no multipart parsing, and now
|
||||||
except ImportError:
|
# carries mu-law-compressed mic audio too, not just video.
|
||||||
HAS_REQUESTS = False
|
# ==========================================
|
||||||
|
TYPE_HELLO = 0x00
|
||||||
|
TYPE_VIDEO = 0x01
|
||||||
|
TYPE_AUDIO = 0x02
|
||||||
|
|
||||||
|
|
||||||
|
def decode_ulaw(q):
|
||||||
|
"""uint8 mu-law samples -> int16 PCM. Must match encode_ulaw() in
|
||||||
|
nao_video_server.py exactly, or this just produces noise."""
|
||||||
|
mu = 255.0
|
||||||
|
y = (q.astype(np.float64) / 255.0) * 2.0 - 1.0
|
||||||
|
x = np.sign(y) * (1.0 / mu) * (np.power(1.0 + mu, np.abs(y)) - 1.0)
|
||||||
|
return np.clip(x * 32768.0, -32768, 32767).astype(np.int16)
|
||||||
|
|
||||||
|
|
||||||
|
def _recv_exact(sock, n):
|
||||||
|
"""Read exactly n bytes from a TCP socket (recv() can return short
|
||||||
|
reads/chunks at any point - never assume one call gives you n)."""
|
||||||
|
buf = b""
|
||||||
|
while len(buf) < n:
|
||||||
|
chunk = sock.recv(n - len(buf))
|
||||||
|
if not chunk:
|
||||||
|
raise IOError("media relay connection closed")
|
||||||
|
buf += chunk
|
||||||
|
return buf
|
||||||
|
|
||||||
|
|
||||||
# ==========================================
|
# ==========================================
|
||||||
# NAO MIC STREAM
|
# NAO MIC STREAM
|
||||||
@@ -89,14 +114,22 @@ class SoundReceiver:
|
|||||||
except: pass
|
except: pass
|
||||||
|
|
||||||
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
||||||
|
self.feed(bytes(buffer))
|
||||||
|
|
||||||
|
def feed(self, raw_pcm_bytes):
|
||||||
|
"""Push one chunk of 16-bit PCM into the playback queue,
|
||||||
|
dropping the oldest chunk on overflow instead of blocking.
|
||||||
|
Used both by the direct qi audio callback (processRemote,
|
||||||
|
above) and by the TCP media relay path (which mu-law-decodes
|
||||||
|
back to PCM before calling this)."""
|
||||||
if not HAS_AUDIO: return
|
if not HAS_AUDIO: return
|
||||||
self.chunks_received += 1
|
self.chunks_received += 1
|
||||||
try:
|
try:
|
||||||
self.queue.put_nowait(bytes(buffer))
|
self.queue.put_nowait(raw_pcm_bytes)
|
||||||
except queue.Full:
|
except queue.Full:
|
||||||
try:
|
try:
|
||||||
self.queue.get_nowait()
|
self.queue.get_nowait()
|
||||||
self.queue.put_nowait(bytes(buffer))
|
self.queue.put_nowait(raw_pcm_bytes)
|
||||||
except: pass
|
except: pass
|
||||||
|
|
||||||
def _playback_loop(self):
|
def _playback_loop(self):
|
||||||
@@ -320,7 +353,7 @@ class LiveMicRelay:
|
|||||||
class NaoTeleop:
|
class NaoTeleop:
|
||||||
def __init__(self, session, nao_ip, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0,
|
def __init__(self, session, nao_ip, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0,
|
||||||
nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_port=5555, relay_rate=16000,
|
nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_port=5555, relay_rate=16000,
|
||||||
video_fps=25, video_http_url=None):
|
video_fps=25, media_relay=None):
|
||||||
self.session = session
|
self.session = session
|
||||||
self.nao_ip = nao_ip
|
self.nao_ip = nao_ip
|
||||||
self.motion = session.service("ALMotion")
|
self.motion = session.service("ALMotion")
|
||||||
@@ -367,7 +400,12 @@ class NaoTeleop:
|
|||||||
|
|
||||||
self.video_scale = video_scale
|
self.video_scale = video_scale
|
||||||
self.video_fps = video_fps
|
self.video_fps = video_fps
|
||||||
self.video_http_url = video_http_url
|
self.media_relay = media_relay
|
||||||
|
# True only if we actually subscribed to ALAudioDevice ourselves
|
||||||
|
# (i.e. not using the relay, where nao_video_server.py owns the
|
||||||
|
# mic subscription instead) - used at shutdown so we don't try
|
||||||
|
# to unsubscribe from something we never subscribed to.
|
||||||
|
self._qi_audio_subscribed = False
|
||||||
self.display_width = int(320 * video_scale)
|
self.display_width = int(320 * video_scale)
|
||||||
self.display_height = int(240 * video_scale)
|
self.display_height = int(240 * video_scale)
|
||||||
|
|
||||||
@@ -410,15 +448,11 @@ class NaoTeleop:
|
|||||||
self.audio_device = None
|
self.audio_device = None
|
||||||
|
|
||||||
def _init_video_maxfps(self):
|
def _init_video_maxfps(self):
|
||||||
if self.video_http_url:
|
if self.media_relay:
|
||||||
if not HAS_REQUESTS:
|
|
||||||
print("❌ --video-http-url given but 'requests' isn't installed (pip install requests)")
|
|
||||||
self.video_client = None
|
|
||||||
return
|
|
||||||
self.video = None
|
self.video = None
|
||||||
self.video_client = "http" # truthy sentinel, unsubscribe() is a no-op for this path
|
self.video_client = "relay" # truthy sentinel, unsubscribe() is a no-op for this path
|
||||||
print("✅ Camera via on-robot MJPEG relay: %s" % self.video_http_url)
|
print(f"✅ Camera + mic via on-robot media relay: {self.media_relay}")
|
||||||
threading.Thread(target=self._video_loop_http, daemon=True).start()
|
threading.Thread(target=self._media_relay_loop, daemon=True).start()
|
||||||
return
|
return
|
||||||
try:
|
try:
|
||||||
self.video = self.session.service("ALVideoDevice")
|
self.video = self.session.service("ALVideoDevice")
|
||||||
@@ -448,45 +482,44 @@ class NaoTeleop:
|
|||||||
except Exception:
|
except Exception:
|
||||||
time.sleep(0.05)
|
time.sleep(0.05)
|
||||||
|
|
||||||
def _video_loop_http(self):
|
def _media_relay_loop(self):
|
||||||
"""Same role as _video_loop, but pulls an already-JPEG-encoded
|
"""Same role as _video_loop, but speaks the lean length-framed
|
||||||
MJPEG stream from nao_video_server.py running on the robot,
|
TCP protocol from nao_video_server.py instead of pulling an
|
||||||
instead of raw RGB frames over the qi RPC session - this is
|
HTTP MJPEG stream - and now demuxes BOTH video and mu-law-
|
||||||
the whole point of doing the re-encoding on-robot rather than
|
compressed mic audio off the same connection, since the mic
|
||||||
shipping raw pixels across the phone tunnel."""
|
subscription lives on the robot too now instead of shipping
|
||||||
boundary = b"--frame"
|
raw PCM through the qi RPC session."""
|
||||||
|
host, _, port_str = self.media_relay.partition(":")
|
||||||
|
port = int(port_str) if port_str else 8000
|
||||||
while not self._video_stop_event.is_set():
|
while not self._video_stop_event.is_set():
|
||||||
|
sock = None
|
||||||
try:
|
try:
|
||||||
resp = requests.get(self.video_http_url, stream=True, timeout=5)
|
sock = socket.create_connection((host, port), timeout=5)
|
||||||
buf = b""
|
sock.settimeout(5.0)
|
||||||
for chunk in resp.iter_content(chunk_size=4096):
|
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||||
if self._video_stop_event.is_set():
|
print(f"✅ Media relay connected: {host}:{port}")
|
||||||
return
|
while not self._video_stop_event.is_set():
|
||||||
buf += chunk
|
msg_type, length = struct.unpack(">BI", _recv_exact(sock, 5))
|
||||||
while True:
|
payload = _recv_exact(sock, length) if length else b""
|
||||||
start = buf.find(boundary)
|
|
||||||
if start == -1:
|
if msg_type == TYPE_VIDEO:
|
||||||
break
|
arr = np.frombuffer(payload, dtype=np.uint8)
|
||||||
hdr_end = buf.find(b"\r\n\r\n", start)
|
|
||||||
if hdr_end == -1:
|
|
||||||
break
|
|
||||||
m = re.search(rb"Content-Length:\s*(\d+)", buf[start:hdr_end])
|
|
||||||
if not m:
|
|
||||||
buf = buf[hdr_end + 4:]
|
|
||||||
continue
|
|
||||||
length = int(m.group(1))
|
|
||||||
frame_start = hdr_end + 4
|
|
||||||
if len(buf) < frame_start + length:
|
|
||||||
break
|
|
||||||
jpeg = buf[frame_start:frame_start + length]
|
|
||||||
buf = buf[frame_start + length:]
|
|
||||||
arr = np.frombuffer(jpeg, dtype=np.uint8)
|
|
||||||
frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
|
frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
|
||||||
if frame is not None:
|
if frame is not None:
|
||||||
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
||||||
self._latest_frame_ts = time.time()
|
self._latest_frame_ts = time.time()
|
||||||
|
elif msg_type == TYPE_AUDIO:
|
||||||
|
if getattr(self, "sound_receiver", None):
|
||||||
|
pcm = decode_ulaw(np.frombuffer(payload, dtype=np.uint8))
|
||||||
|
self.sound_receiver.feed(pcm.tobytes())
|
||||||
|
# TYPE_HELLO carries {"rate", "channels"} for future use - nothing to do with it yet.
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f"video relay error: {e}")
|
print(f"media relay error: {e}")
|
||||||
|
finally:
|
||||||
|
if sock:
|
||||||
|
try: sock.close()
|
||||||
|
except: pass
|
||||||
|
if not self._video_stop_event.is_set():
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
def get_image(self):
|
def get_image(self):
|
||||||
@@ -498,13 +531,20 @@ class NaoTeleop:
|
|||||||
def _init_mic_stream(self):
|
def _init_mic_stream(self):
|
||||||
if not self.audio_device: return
|
if not self.audio_device: return
|
||||||
try:
|
try:
|
||||||
|
# Playback machinery is needed either way - only the source
|
||||||
|
# of audio chunks differs (qi pub/sub below, vs mu-law
|
||||||
|
# chunks arriving over the media relay in _media_relay_loop).
|
||||||
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
||||||
pyaudio_instance=self._pyaudio)
|
pyaudio_instance=self._pyaudio)
|
||||||
|
if self.media_relay:
|
||||||
|
print(f"✅ Mic stream ready (gain={self.mic_gain}x, via media relay - compressed)")
|
||||||
|
return
|
||||||
self.session.registerService(self.audio_service_name, self.sound_receiver)
|
self.session.registerService(self.audio_service_name, self.sound_receiver)
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
||||||
self.audio_device.subscribe(self.audio_service_name)
|
self.audio_device.subscribe(self.audio_service_name)
|
||||||
print(f"✅ Mic stream ready (gain={self.mic_gain}x)")
|
self._qi_audio_subscribed = True
|
||||||
|
print(f"✅ Mic stream ready (gain={self.mic_gain}x, uncompressed direct - use --media-relay on a tunneled link)")
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f"❌ Mic init failed: {e}")
|
print(f"❌ Mic init failed: {e}")
|
||||||
|
|
||||||
@@ -930,7 +970,7 @@ class NaoTeleop:
|
|||||||
if getattr(self, "mic_relay", None):
|
if getattr(self, "mic_relay", None):
|
||||||
self._safe("mic relay stop", self.mic_relay.stop)
|
self._safe("mic relay stop", self.mic_relay.stop)
|
||||||
self._safe("mic relay close", self.mic_relay.close)
|
self._safe("mic relay close", self.mic_relay.close)
|
||||||
if self.audio_device:
|
if self.audio_device and self._qi_audio_subscribed:
|
||||||
self._safe("audio unsubscribe", lambda: self.audio_device.unsubscribe(self.audio_service_name))
|
self._safe("audio unsubscribe", lambda: self.audio_device.unsubscribe(self.audio_service_name))
|
||||||
if getattr(self, "sound_receiver", None):
|
if getattr(self, "sound_receiver", None):
|
||||||
self._safe("sound receiver close", self.sound_receiver.close)
|
self._safe("sound receiver close", self.sound_receiver.close)
|
||||||
@@ -956,12 +996,15 @@ if __name__ == "__main__":
|
|||||||
help="Camera fps requested from NAO. Video is uncompressed RGB at "
|
help="Camera fps requested from NAO. Video is uncompressed RGB at "
|
||||||
"160x120, so this is roughly 460KB/s per fps - lower it "
|
"160x120, so this is roughly 460KB/s per fps - lower it "
|
||||||
"(e.g. 10-12) when tunneling over a constrained link. "
|
"(e.g. 10-12) when tunneling over a constrained link. "
|
||||||
"Ignored when --video-http-url is set (the server controls fps).")
|
"Ignored when --media-relay is set (the server controls fps).")
|
||||||
parser.add_argument("--video-http-url", type=str, default=None,
|
parser.add_argument("--media-relay", type=str, default=None,
|
||||||
help="URL of the on-robot MJPEG relay (nao_video_server.py), e.g. "
|
help="host:port of the on-robot media relay (nao_video_server.py), "
|
||||||
"http://127.0.0.1:8000/video when tunneled through the phone. "
|
"e.g. 127.0.0.1:8000 when tunneled through the phone. When set, "
|
||||||
"When set, video comes from this compressed HTTP stream instead "
|
"BOTH video and mic audio come compressed over this one TCP "
|
||||||
"of raw frames over the qi session - much lighter over a relay.")
|
"connection (JPEG + mu-law) instead of raw frames/PCM over the "
|
||||||
|
"qi session - use this on a tunneled/shoddy link. --mic-channel "
|
||||||
|
"is ignored in this mode (set MIC_CHANNEL in nao_video_server.py "
|
||||||
|
"instead, since the mic subscription lives there now).")
|
||||||
parser.add_argument("--nao-ssh-user", type=str, default="nao",
|
parser.add_argument("--nao-ssh-user", type=str, default="nao",
|
||||||
help="SSH login for the live mic relay (push-to-talk)")
|
help="SSH login for the live mic relay (push-to-talk)")
|
||||||
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
||||||
@@ -989,7 +1032,7 @@ if __name__ == "__main__":
|
|||||||
mic_gain=args.mic_gain,
|
mic_gain=args.mic_gain,
|
||||||
video_scale=args.video_scale,
|
video_scale=args.video_scale,
|
||||||
video_fps=args.video_fps,
|
video_fps=args.video_fps,
|
||||||
video_http_url=args.video_http_url,
|
media_relay=args.media_relay,
|
||||||
nao_ssh_user=args.nao_ssh_user,
|
nao_ssh_user=args.nao_ssh_user,
|
||||||
nao_ssh_password=args.nao_ssh_password,
|
nao_ssh_password=args.nao_ssh_password,
|
||||||
nao_ssh_port=args.nao_ssh_port,
|
nao_ssh_port=args.nao_ssh_port,
|
||||||
|
|||||||
@@ -1,4 +1,4 @@
|
|||||||
#!/bin/bash
|
#!/bin/bash
|
||||||
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
||||||
#python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
#python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
||||||
python3.11 naowalk.py --ip 127.0.0.1 --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 2222 --video-http-url http://127.0.0.1:8000/video
|
python3.11 naowalk.py --ip 127.0.0.1 --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 2222 --media-relay 127.0.0.1:8000
|
||||||
|
|||||||
Reference in New Issue
Block a user