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
naowalk.py
+103 -60
View File
@@ -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,