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.
|
||||
|
||||
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__":
|
||||
|
||||
+103
-60
@@ -13,7 +13,7 @@ import pygame
|
||||
import subprocess
|
||||
import os
|
||||
import json
|
||||
import re
|
||||
import struct
|
||||
import socket
|
||||
|
||||
# Import the new music module
|
||||
@@ -33,12 +33,37 @@ try:
|
||||
except ImportError:
|
||||
HAS_SSH = False
|
||||
|
||||
# --- HTTP client for the on-robot MJPEG relay (see nao_video_server.py) ---
|
||||
try:
|
||||
import requests
|
||||
HAS_REQUESTS = True
|
||||
except ImportError:
|
||||
HAS_REQUESTS = False
|
||||
# ==========================================
|
||||
# MEDIA RELAY WIRE PROTOCOL (see nao_video_server.py)
|
||||
# One TCP connection, length-framed: [1 byte type][4 byte length][payload].
|
||||
# Replaces the old HTTP MJPEG pull - no multipart parsing, and now
|
||||
# carries mu-law-compressed mic audio too, not just video.
|
||||
# ==========================================
|
||||
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
|
||||
@@ -89,14 +114,22 @@ class SoundReceiver:
|
||||
except: pass
|
||||
|
||||
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
|
||||
self.chunks_received += 1
|
||||
try:
|
||||
self.queue.put_nowait(bytes(buffer))
|
||||
self.queue.put_nowait(raw_pcm_bytes)
|
||||
except queue.Full:
|
||||
try:
|
||||
self.queue.get_nowait()
|
||||
self.queue.put_nowait(bytes(buffer))
|
||||
self.queue.put_nowait(raw_pcm_bytes)
|
||||
except: pass
|
||||
|
||||
def _playback_loop(self):
|
||||
@@ -320,7 +353,7 @@ class LiveMicRelay:
|
||||
class NaoTeleop:
|
||||
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,
|
||||
video_fps=25, video_http_url=None):
|
||||
video_fps=25, media_relay=None):
|
||||
self.session = session
|
||||
self.nao_ip = nao_ip
|
||||
self.motion = session.service("ALMotion")
|
||||
@@ -367,7 +400,12 @@ class NaoTeleop:
|
||||
|
||||
self.video_scale = video_scale
|
||||
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_height = int(240 * video_scale)
|
||||
|
||||
@@ -410,15 +448,11 @@ class NaoTeleop:
|
||||
self.audio_device = None
|
||||
|
||||
def _init_video_maxfps(self):
|
||||
if self.video_http_url:
|
||||
if not HAS_REQUESTS:
|
||||
print("❌ --video-http-url given but 'requests' isn't installed (pip install requests)")
|
||||
self.video_client = None
|
||||
return
|
||||
if self.media_relay:
|
||||
self.video = None
|
||||
self.video_client = "http" # truthy sentinel, unsubscribe() is a no-op for this path
|
||||
print("✅ Camera via on-robot MJPEG relay: %s" % self.video_http_url)
|
||||
threading.Thread(target=self._video_loop_http, daemon=True).start()
|
||||
self.video_client = "relay" # truthy sentinel, unsubscribe() is a no-op for this path
|
||||
print(f"✅ Camera + mic via on-robot media relay: {self.media_relay}")
|
||||
threading.Thread(target=self._media_relay_loop, daemon=True).start()
|
||||
return
|
||||
try:
|
||||
self.video = self.session.service("ALVideoDevice")
|
||||
@@ -448,45 +482,44 @@ class NaoTeleop:
|
||||
except Exception:
|
||||
time.sleep(0.05)
|
||||
|
||||
def _video_loop_http(self):
|
||||
"""Same role as _video_loop, but pulls an already-JPEG-encoded
|
||||
MJPEG stream from nao_video_server.py running on the robot,
|
||||
instead of raw RGB frames over the qi RPC session - this is
|
||||
the whole point of doing the re-encoding on-robot rather than
|
||||
shipping raw pixels across the phone tunnel."""
|
||||
boundary = b"--frame"
|
||||
def _media_relay_loop(self):
|
||||
"""Same role as _video_loop, but speaks the lean length-framed
|
||||
TCP protocol from nao_video_server.py instead of pulling an
|
||||
HTTP MJPEG stream - and now demuxes BOTH video and mu-law-
|
||||
compressed mic audio off the same connection, since the mic
|
||||
subscription lives on the robot too now instead of shipping
|
||||
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():
|
||||
sock = None
|
||||
try:
|
||||
resp = requests.get(self.video_http_url, stream=True, timeout=5)
|
||||
buf = b""
|
||||
for chunk in resp.iter_content(chunk_size=4096):
|
||||
if self._video_stop_event.is_set():
|
||||
return
|
||||
buf += chunk
|
||||
while True:
|
||||
start = buf.find(boundary)
|
||||
if start == -1:
|
||||
break
|
||||
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)
|
||||
sock = socket.create_connection((host, port), timeout=5)
|
||||
sock.settimeout(5.0)
|
||||
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||
print(f"✅ Media relay connected: {host}:{port}")
|
||||
while not self._video_stop_event.is_set():
|
||||
msg_type, length = struct.unpack(">BI", _recv_exact(sock, 5))
|
||||
payload = _recv_exact(sock, length) if length else b""
|
||||
|
||||
if msg_type == TYPE_VIDEO:
|
||||
arr = np.frombuffer(payload, dtype=np.uint8)
|
||||
frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
|
||||
if frame is not None:
|
||||
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
||||
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:
|
||||
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)
|
||||
|
||||
def get_image(self):
|
||||
@@ -498,13 +531,20 @@ class NaoTeleop:
|
||||
def _init_mic_stream(self):
|
||||
if not self.audio_device: return
|
||||
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,
|
||||
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)
|
||||
time.sleep(0.5)
|
||||
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
||||
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:
|
||||
print(f"❌ Mic init failed: {e}")
|
||||
|
||||
@@ -930,7 +970,7 @@ class NaoTeleop:
|
||||
if getattr(self, "mic_relay", None):
|
||||
self._safe("mic relay stop", self.mic_relay.stop)
|
||||
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))
|
||||
if getattr(self, "sound_receiver", None):
|
||||
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 "
|
||||
"160x120, so this is roughly 460KB/s per fps - lower it "
|
||||
"(e.g. 10-12) when tunneling over a constrained link. "
|
||||
"Ignored when --video-http-url is set (the server controls fps).")
|
||||
parser.add_argument("--video-http-url", type=str, default=None,
|
||||
help="URL of the on-robot MJPEG relay (nao_video_server.py), e.g. "
|
||||
"http://127.0.0.1:8000/video when tunneled through the phone. "
|
||||
"When set, video comes from this compressed HTTP stream instead "
|
||||
"of raw frames over the qi session - much lighter over a relay.")
|
||||
"Ignored when --media-relay is set (the server controls fps).")
|
||||
parser.add_argument("--media-relay", type=str, default=None,
|
||||
help="host:port of the on-robot media relay (nao_video_server.py), "
|
||||
"e.g. 127.0.0.1:8000 when tunneled through the phone. When set, "
|
||||
"BOTH video and mic audio come compressed over this one TCP "
|
||||
"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",
|
||||
help="SSH login for the live mic relay (push-to-talk)")
|
||||
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
||||
@@ -989,7 +1032,7 @@ if __name__ == "__main__":
|
||||
mic_gain=args.mic_gain,
|
||||
video_scale=args.video_scale,
|
||||
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_password=args.nao_ssh_password,
|
||||
nao_ssh_port=args.nao_ssh_port,
|
||||
|
||||
@@ -1,4 +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 --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