Compare commits

...

3 Commits

10 changed files with 731 additions and 144 deletions
+4
View File
@@ -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.
+327
View File
@@ -0,0 +1,327 @@
# -*- 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()
-101
View File
@@ -1,101 +0,0 @@
#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
"""
Standalone NAO audio diagnostic - deliberately bypasses naowalk.py entirely.
Run this FIRST when troubleshooting "I can't hear the robot's mic". It tests
each link in the chain independently, so a failure tells you exactly where
the problem is instead of "somewhere in 300 lines of PyAudio/threading code":
TEST 1 - Front mic hardware energy.
Uses ALAudioDevice's built-in energy meter. No PyAudio, no
network audio streaming, no laptop audio stack involved at all.
This is the lowest-level check of "does the mic itself work".
TEST 2 - Local on-robot recording.
Records straight to a .wav file on the robot's own disk, then
you pull it off and play it normally. This bypasses the live
network stream, the SoundReceiver/queue/threading code, and
your laptop's PyAudio output path - all at once.
TEST 3 - Robot's own speaker.
Plays a tone through the ROBOT's speaker (not your laptop), to
confirm robot-side audio output is fine, independent of any of
the mic-listening code.
If TEST 1 shows no energy change when you make noise: mic hardware/mute/
environment issue, upstream of any code in this project.
If TEST 1 works but TEST 2's file is silent: something is wrong with the
recording/robot-audio-stack itself, still nothing to do with naowalk.py.
If TEST 2's file has clear audio: the mic and robot are fine, and the bug
is specifically in naowalk.py's live-streaming path (network delivery,
PyAudio device selection, etc.) - go back to the diagnostics already
built into naowalk.py (device list / test beep / peak level) to isolate it
further.
Usage:
python3 nao_audio_diagnostic.py --ip spike.local --port 9559
"""
import qi
import argparse
import time
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1")
parser.add_argument("--port", type=int, default=9559)
args = parser.parse_args()
session = qi.Session()
session.connect(f"tcp://{args.ip}:{args.port}")
print(f"✅ Connected to {args.ip}")
audio = session.service("ALAudioDevice")
# --- TEST 1: mic hardware energy, all four channels ------------------------
print("\n=== TEST 1: Mic hardware energy (all 4 mics) ===")
print("Make noise near the robot's head (clap, talk right up close to it) for 10s...")
audio.enableEnergyComputation()
front_readings, rear_readings, left_readings, right_readings = [], [], [], []
for i in range(10):
f, r, l, rt = (audio.getFrontMicEnergy(), audio.getRearMicEnergy(),
audio.getLeftMicEnergy(), audio.getRightMicEnergy())
front_readings.append(f); rear_readings.append(r); left_readings.append(l); right_readings.append(rt)
print(f" t={i}s front={f:.2f} rear={r:.2f} left={l:.2f} right={rt:.2f}")
time.sleep(1)
audio.disableEnergyComputation()
results = {"front": front_readings, "rear": rear_readings, "left": left_readings, "right": right_readings}
flat = [name for name, vals in results.items() if max(vals) - min(vals) < 2]
if len(flat) == 4:
print("⚠️ ALL FOUR mics are flat. This points to something systemic - e.g. audio "
"inputs closed (closeAudioInputs() called somewhere and never reopened), "
"or a low-level hardware/driver fault - rather than one damaged mic.")
elif flat:
print(f"⚠️ Flat mic(s): {', '.join(flat)}. The others responded normally, which "
f"points to a fault specific to {', '.join(flat)} rather than the audio "
f"pipeline as a whole (that pipeline is clearly working for the others).")
else:
print("✅ All four mics responded to noise - hardware capture is fine across the board.")
# --- TEST 2: local on-robot recording ---------------------------------------
print("\n=== TEST 2: Local on-robot recording ===")
remote_path = "/home/nao/mic_test.wav"
print(f"Recording 5s directly to {remote_path} on the robot itself "
f"(this bypasses the network stream and your laptop's audio stack entirely)...")
audio.startMicrophonesRecording(remote_path)
time.sleep(5)
audio.stopMicrophonesRecording()
print(f"Done. Pull it off and play it with:")
print(f" scp nao@{args.ip}:{remote_path} .")
print(f" (then open mic_test.wav in any media player - it's 4-channel, so check all channels)")
print("If that file has clearly audible sound, the mic and robot are fine and the")
print("bug is specifically in naowalk.py's live-streaming path.")
# --- TEST 3: robot's own speaker, via TTS (version-agnostic) ---------------
print("\n=== TEST 3: Robot's own speaker ===")
input("Press Enter to have the robot say a test phrase out loud...")
session.service("ALTextToSpeech").say("Testing 1 2 3, can you hear me.")
print("If you heard the robot speak just now, its speaker output is fine "
"(this is unrelated to hearing its mic on your laptop, but rules out a robot-wide audio fault).")
print("\nDone. Report back which test(s) failed and we'll know exactly where to look.")
+6 -3
View File
@@ -9,15 +9,17 @@ import time
import threading
class NaoMusicPlayer:
def __init__(self, session, nao_ip):
def __init__(self, session, nao_ip, ssh_port=22):
self.session = session
self.player = session.service("ALAudioPlayer")
self.nao_ip = nao_ip
self.ssh_port = ssh_port
self.music_dir = "/home/nao/music"
self.local_path = "/tmp/song.mp3"
self.remote_path = f"{self.music_dir}/song.mp3"
try:
subprocess.run(["sshpass", "-p", "nao", "ssh", f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"],
subprocess.run(["sshpass", "-p", "nao", "ssh", "-p", str(self.ssh_port),
f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"],
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
except: pass
self.current_file = None
@@ -110,7 +112,8 @@ class NaoMusicPlayer:
t = threading.Thread(target=ticker, daemon=True)
t.start()
scp_result = subprocess.run(
["sshpass", "-p", "nao", "scp", self.local_path, f"nao@{self.nao_ip}:{self.remote_path}"],
["sshpass", "-p", "nao", "scp", "-P", str(self.ssh_port),
self.local_path, f"nao@{self.nao_ip}:{self.remote_path}"],
capture_output=True, text=True, timeout=60
)
stop_ticker.set()
+390 -39
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,6 +33,38 @@ try:
except ImportError:
HAS_SSH = 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
# ==========================================
@@ -82,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):
@@ -163,11 +203,12 @@ class LiveMicRelay:
nc so it's ready for the next press. close() (call once at shutdown)
tears down the persistent SSH connection and kills the remote loop.
"""
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao",
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22,
port=5555, rate=16000, input_device_index=None, pyaudio_instance=None):
self.nao_ip = nao_ip
self.ssh_user = ssh_user
self.ssh_password = ssh_password
self.ssh_port = ssh_port
self.port = port
self.rate = rate
self.input_device_index = input_device_index
@@ -199,7 +240,7 @@ class LiveMicRelay:
try:
self._ssh_client = paramiko.SSHClient()
self._ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
self._ssh_client.connect(self.nao_ip, username=self.ssh_user,
self._ssh_client.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
password=self.ssh_password, timeout=5)
self._ssh_channel = self._ssh_client.get_transport().open_session()
# A pty ties the remote process group to this channel, so
@@ -306,12 +347,140 @@ class LiveMicRelay:
self._cleanup()
print("🎙️ Talk relay: stopped")
# ==========================================
# ON-ROBOT VIDEO/AUDIO RELAY SERVER (auto-deploy)
# ==========================================
class VideoServerLauncher:
"""
SCPs nao_video_server.py to the robot's home directory over SFTP and
launches it via a persistent, pty-backed SSH session - so the user
never has to manually copy the file over or SSH in to start/stop it.
Uses the same trick as LiveMicRelay: a pty ties the remote process
group to the SSH channel, so close() (killing the channel) reliably
kills the video server too instead of leaving it orphaned on the
robot, still holding VIDEO_PORT/AUDIO_PORT, for next time.
"""
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22,
local_path=None, remote_filename="nao_video_server.py"):
self.nao_ip = nao_ip
self.ssh_user = ssh_user
self.ssh_password = ssh_password
self.ssh_port = ssh_port
self.local_path = local_path or os.path.join(
os.path.dirname(os.path.abspath(__file__)), remote_filename)
self.remote_filename = remote_filename
self.ready = False
self._ssh_client = None
self._channel = None
self._drain_thread = None
def deploy_and_start(self):
if not HAS_SSH:
print("🎥 Video server auto-deploy needs paramiko: pip install paramiko")
return False
if not os.path.isfile(self.local_path):
print(f"🎥 Video server auto-deploy: can't find {self.local_path}")
return False
try:
self._ssh_client = paramiko.SSHClient()
self._ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
self._ssh_client.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
password=self.ssh_password, timeout=10)
# Best-effort: a previous session that didn't get to call
# close() cleanly (laptop lost network, crashed, etc.) can
# leave a stale instance squatting on the ports - clear it
# before starting a fresh one.
try:
_, out, _ = self._ssh_client.exec_command(
f"pkill -f {self.remote_filename}", timeout=5)
out.channel.recv_exit_status()
time.sleep(0.3)
except Exception:
pass
sftp = self._ssh_client.open_sftp()
try:
remote_home = sftp.normalize(".") # robot's home dir
remote_path = remote_home + "/" + self.remote_filename
print(f"🎥 Copying {self.remote_filename} -> {self.nao_ip}:{remote_path}")
sftp.put(self.local_path, remote_path)
finally:
sftp.close()
self._channel = self._ssh_client.get_transport().open_session()
self._channel.get_pty()
remote_cmd = (
"export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages && "
"export LD_LIBRARY_PATH=/opt/aldebaran/lib && "
f"python2 {remote_path}"
)
self._channel.exec_command(remote_cmd)
self._drain_thread = threading.Thread(target=self._drain_output, daemon=True)
self._drain_thread.start()
time.sleep(1.0) # give it a moment to bind before the client tries to connect
if self._channel.exit_status_ready():
print("🎥 Video server exited immediately - check the log lines above")
self.ready = False
return False
self.ready = True
print("🎥 Video server deployed and running on robot")
return True
except Exception as e:
print(f"🎥 Video server auto-deploy failed: {e}")
self.ready = False
return False
def _drain_output(self):
"""Keeps reading the remote process's stdout/stderr so its pty
buffer never fills up and stalls the server, and so its own
status prints (quality tier changes, client connects) surface
locally too."""
chan = self._channel
while chan and not chan.closed:
try:
got = False
if chan.recv_ready():
data = chan.recv(4096)
if data:
print(f"🎥 [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}")
got = True
if chan.recv_stderr_ready():
data = chan.recv_stderr(4096)
if data:
print(f"🎥 [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}")
got = True
if not got:
time.sleep(0.1)
except Exception:
break
def close(self):
"""Kill the pty-backed channel (which kills the remote python
process with it) and tear down the SSH connection. Call once at
app shutdown."""
if self._channel:
try: self._channel.close()
except: pass
self._channel = None
if self._ssh_client:
try: self._ssh_client.close()
except: pass
self._ssh_client = None
self.ready = False
# ==========================================
# MAIN TELEOP
# ==========================================
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", 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, media_relay=None, deploy_video_server=True, video_server_path=None):
self.session = session
self.nao_ip = nao_ip
self.motion = session.service("ALMotion")
@@ -322,7 +491,7 @@ class NaoTeleop:
except:
self.battery = None
self.music = NaoMusicPlayer(session, nao_ip)
self.music = NaoMusicPlayer(session, nao_ip, ssh_port=nao_ssh_port)
self.music_results = []
self.selected_result = 0
@@ -338,6 +507,7 @@ class NaoTeleop:
self._pyaudio = pyaudio.PyAudio() if HAS_AUDIO else None
self.mic_relay = LiveMicRelay(nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password,
ssh_port=nao_ssh_port,
port=relay_port, rate=relay_rate, pyaudio_instance=self._pyaudio)
self.ptt_active = False
@@ -356,9 +526,37 @@ class NaoTeleop:
self.music_search_pending = False
self.video_scale = video_scale
self.video_fps = video_fps
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)
# Video and movement are decoupled onto their own threads (see
# _init_video_maxfps / run) so a stalled network RPC to the robot
# never blocks keyboard input or the render loop.
self._latest_frame = None
self._latest_frame_ts = 0.0
self._video_stop_event = threading.Event()
self._desired_velocity = (0.0, 0.0, 0.0)
self._movement_stop_event = threading.Event()
self._movement_thread = None
# If we're using the on-robot media relay, deploy+launch
# nao_video_server.py over SSH now (before _init_video_maxfps
# tries to connect to it) instead of requiring it to already be
# running manually.
self.video_server_launcher = None
if self.media_relay and deploy_video_server:
self.video_server_launcher = VideoServerLauncher(
nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password,
ssh_port=nao_ssh_port, local_path=video_server_path)
self.video_server_launcher.deploy_and_start()
self._init_audio_device()
self._init_video_maxfps()
if HAS_AUDIO:
@@ -388,41 +586,133 @@ class NaoTeleop:
self.audio_device = None
def _init_video_maxfps(self):
if self.media_relay:
self.video = None
self.video_client = "relay" # truthy sentinel, unsubscribe() is a no-op for this path
print(f"✅ Camera via on-robot media relay: {self.media_relay}")
threading.Thread(target=self._video_relay_loop, daemon=True).start()
return
try:
self.video = self.session.service("ALVideoDevice")
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25)
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, self.video_fps)
print("✅ Camera ready")
threading.Thread(target=self._video_loop, daemon=True).start()
except:
print("❌ Camera not available")
self.video_client = None
def _video_loop(self):
"""Runs on its own thread for the app's lifetime. getImageRemote()
is a blocking RPC over the network - if a call hangs during a
WiFi hiccup, this thread just sits waiting on it. The render loop
never touches this call directly; it only ever reads whatever
self._latest_frame was last successfully set to, so a stalled
fetch here can no longer freeze keyboard input or movement."""
while not self._video_stop_event.is_set():
try:
image = self.video.getImageRemote(self.video_client)
if image and len(image) >= 7:
w, h = image[0], image[1]
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
self._latest_frame_ts = time.time()
except Exception:
time.sleep(0.05)
def _video_relay_loop(self):
"""Same role as _video_loop, but speaks the lean length-framed
TCP protocol from nao_video_server.py instead of pulling raw
frames over the qi session. Video-only connection - it used to
share one socket with audio, but a slow video send could then
block audio behind it, so they're split (see the audio port,
media_relay port + 1, handled by _audio_relay_loop)."""
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:
sock = socket.create_connection((host, port), timeout=5)
sock.settimeout(5.0)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
print(f"✅ Video 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()
# TYPE_HELLO: nothing to do with it yet, just skip.
except Exception as e:
print(f"video relay error: {e}")
finally:
if sock:
try: sock.close()
except: pass
if not self._video_stop_event.is_set():
time.sleep(0.5)
def _audio_relay_loop(self):
"""Mic audio's own connection - see _video_relay_loop for why
this is split off rather than sharing video's socket. Connects
to media_relay's host:port+1 by convention (matches AUDIO_PORT
= VIDEO_PORT + 1 in nao_video_server.py)."""
host, _, port_str = self.media_relay.partition(":")
video_port = int(port_str) if port_str else 8000
port = video_port + 1
while not self._video_stop_event.is_set():
sock = None
try:
sock = socket.create_connection((host, port), timeout=5)
sock.settimeout(5.0)
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
print(f"✅ Audio 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_AUDIO and getattr(self, "sound_receiver", None):
pcm = decode_ulaw(np.frombuffer(payload, dtype=np.uint8))
self.sound_receiver.feed(pcm.tobytes())
# TYPE_HELLO: nothing to do with it yet, just skip.
except Exception as e:
print(f"audio 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):
"""Non-blocking: returns whatever frame the background video
thread most recently captured (or None before the first one
arrives). Never makes a network call itself."""
return self._latest_frame
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 audio relay connection).
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, own connection)")
threading.Thread(target=self._audio_relay_loop, daemon=True).start()
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}")
def get_image(self):
if not hasattr(self, 'video') or not self.video_client:
return None
try:
image = self.video.getImageRemote(self.video_client)
if image and len(image) >= 7:
w, h = image[0], image[1]
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
except:
pass
return None
def wave_gesture(self):
if self.typing_mode: return
print("🤚 Waving hello...")
@@ -552,6 +842,32 @@ class NaoTeleop:
self.update_title()
self.last_batt_check = now
def _movement_loop(self):
"""Runs on its own thread for the app's lifetime, sending whatever
velocity the render loop most recently recorded in
self._desired_velocity. moveToward/stopMove are blocking network
RPCs - if one hangs during a WiFi hiccup, only this thread stalls;
the render loop keeps reading keys and updating the target
velocity so the very next send (as soon as the network unblocks)
reflects your latest input, not a stale one from before the drop.
"""
stable_config = [
["StepHeight", 0.02],
["MaxStepFrequency", 0.5],
["TorsoWy", 0.08],
["MaxStepX", 0.03]
]
while not self._movement_stop_event.is_set():
x, y, theta = self._desired_velocity
try:
if abs(x) > 0.05 or abs(y) > 0.05 or abs(theta) > 0.05:
self.motion.moveToward(x, y, theta, stable_config)
else:
self.motion.stopMove()
except Exception as e:
print(f"Move error: {e}")
time.sleep(0.05) # ~20Hz send rate
def run(self):
print("🤖 Enabling stiffness (no wakeUp stand-up)...")
self.motion.setStiffnesses("Body", 1.0)
@@ -570,6 +886,9 @@ class NaoTeleop:
if getattr(self, "mic_relay", None):
threading.Thread(target=self.mic_relay.connect, daemon=True).start()
self._movement_thread = threading.Thread(target=self._movement_loop, daemon=True)
self._movement_thread.start()
print("\n🎮 Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | 2=Cena | 3=6-7 | M=Music search | X=Stop | -/==Volume | T=TTS | V=Push-to-talk (hold) | ESC=Quit")
try:
@@ -698,19 +1017,11 @@ class NaoTeleop:
if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = self.walk_speed * 0.8
if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -self.walk_speed * 0.8
try:
if abs(x) > 0.05 or abs(theta) > 0.05:
stable_config = [
["StepHeight", 0.02],
["MaxStepFrequency", 0.5],
["TorsoWy", 0.08],
["MaxStepX", 0.03]
]
self.motion.moveToward(x, y, theta, stable_config)
else:
self.motion.stopMove()
except Exception as e:
print(f"Move error: {e}")
# Just record intent here - the movement thread
# (see _movement_loop) is what actually calls
# moveToward/stopMove, so a stalled network RPC
# can't stop us reading the next key state.
self._desired_velocity = (x, y, theta)
self.handle_head_movement()
frame = self.get_image()
@@ -719,6 +1030,9 @@ class NaoTeleop:
rgb = cv2.cvtColor(up, cv2.COLOR_BGR2RGB)
surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1))
self.screen.blit(surf, (0, 0))
if time.time() - self._latest_frame_ts > 1.0:
warn = self.font.render("⚠ VIDEO STALE - reconnecting...", True, (255, 60, 60))
self.screen.blit(warn, (10, self.display_height - 30))
else:
self.screen.fill((10, 10, 30))
@@ -812,16 +1126,22 @@ class NaoTeleop:
print(f"\u26a0\ufe0f Frame error (continuing): {e}")
finally:
print("🛑 Shutting down...")
self._movement_stop_event.set()
if self._movement_thread:
self._movement_thread.join(timeout=1.0)
self._video_stop_event.set()
self._safe("stopMove", self.motion.stopMove)
self._safe("music stop", self.music.stop)
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 getattr(self, "video_server_launcher", None):
self._safe("video server stop", self.video_server_launcher.close)
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)
if hasattr(self, 'video') and self.video_client:
if hasattr(self, 'video') and self.video is not None and self.video_client:
self._safe("video unsubscribe", lambda: self.video.unsubscribe(self.video_client))
if getattr(self, "_pyaudio", None):
self._safe("pyaudio terminate", self._pyaudio.terminate)
@@ -839,14 +1159,40 @@ if __name__ == "__main__":
parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left")
parser.add_argument("--mic-gain", type=float, default=8.0)
parser.add_argument("--video-scale", type=float, default=2.0)
parser.add_argument("--video-fps", type=int, default=25,
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 --media-relay is set (the server controls fps).")
parser.add_argument("--media-relay", type=str, default=None,
help="host:port to reach the on-robot media relay (nao_video_server.py) "
"at, e.g. 127.0.0.1:8000 when tunneled through the phone. Setting "
"this also makes naowalk auto-scp nao_video_server.py to the "
"robot's home dir over SSH and launch it (see --no-deploy-video-"
"server to skip that and connect to one you started yourself). "
"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",
help="SSH password for the live mic relay (push-to-talk)")
parser.add_argument("--nao-ssh-port", type=int, default=22,
help="SSH port for the robot (music upload + push-to-talk). "
"Set this to your local forwarded port, e.g. 2222, when tunneling.")
parser.add_argument("--relay-port", type=int, default=5555,
help="TCP port used to stream mic audio to the robot's nc listener")
parser.add_argument("--relay-rate", type=int, default=16000,
help="Sample rate for the live mic relay - try 48000 if audio sounds sped up/slow")
parser.add_argument("--no-deploy-video-server", action="store_true",
help="Don't auto-scp/launch nao_video_server.py on the robot when "
"--media-relay is set - use this if you're already running it "
"yourself and just want naowalk to connect to it.")
parser.add_argument("--video-server-path", type=str, default=None,
help="Local path to nao_video_server.py to deploy (default: the "
"copy sitting next to naowalk.py)")
args = parser.parse_args()
session = qi.Session()
@@ -862,7 +1208,12 @@ if __name__ == "__main__":
mic_channel=MIC_CHANNELS[args.mic_channel],
mic_gain=args.mic_gain,
video_scale=args.video_scale,
video_fps=args.video_fps,
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,
relay_port=args.relay_port,
relay_rate=args.relay_rate).run()
relay_rate=args.relay_rate,
deploy_video_server=not args.no_deploy_video_server,
video_server_path=args.video_server_path).run()
+3 -1
View File
@@ -1,3 +1,5 @@
#!/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 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
python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 22 --media-relay spike.local:8000
+1
View File
@@ -0,0 +1 @@
ssh -N -L 9561:192.168.0.100:9561 -L 2222:192.168.0.100:22 -L 8000:192.168.0.100:8000 root@10.196.124.46