diff --git a/nao_video_server.py b/nao_video_server.py index c667e04..0a8ae97 100644 --- a/nao_video_server.py +++ b/nao_video_server.py @@ -3,8 +3,13 @@ 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 ONE persistent, length-framed TCP stream -carrying both compressed video (JPEG) and compressed audio (mu-law). +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 @@ -18,18 +23,20 @@ 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`): +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 {"rate": 16000, "channels": 1} - VIDEO payload: JPEG bytes - AUDIO payload: mu-law encoded 8-bit samples (mono) + 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. Audio is prioritized over video each pass, since gaps in -audio are far more noticeable than a slightly stale picture. +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: @@ -54,7 +61,9 @@ import qi # ---- tunables ---- NAOQI_PORT = 9561 # local qi session port (confirmed working) -TCP_PORT = 8000 # forward this one port through the phone tunnel +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 @@ -209,66 +218,82 @@ class MicGrabber(object): pass -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 _send(conn, msg_type, payload): + conn.sendall(struct.pack(">BI", msg_type, len(payload))) + conn.sendall(payload) - 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")) +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_video_send = 0.0 - while True: - did_something = False + 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 - 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 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(): @@ -284,9 +309,12 @@ def main(): except Exception as e: print("mic capture unavailable (%s) - video only" % e) - server = MediaServer(grabber, mic) + video_thread = threading.Thread(target=serve_video, args=(grabber, "0.0.0.0", VIDEO_PORT)) + video_thread.daemon = True + video_thread.start() + try: - server.serve_forever("0.0.0.0", TCP_PORT) + serve_audio(mic, "0.0.0.0", AUDIO_PORT) except KeyboardInterrupt: pass finally: diff --git a/naowalk.py b/naowalk.py index 3a0e324..57c6aec 100644 --- a/naowalk.py +++ b/naowalk.py @@ -347,13 +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", nao_ssh_port=22, relay_port=5555, relay_rate=16000, - video_fps=25, media_relay=None): + 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") @@ -419,6 +546,17 @@ class NaoTeleop: 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: @@ -451,8 +589,8 @@ class NaoTeleop: if self.media_relay: self.video = None 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() + 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") @@ -482,13 +620,13 @@ class NaoTeleop: except Exception: time.sleep(0.05) - def _media_relay_loop(self): + 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 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.""" + 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(): @@ -497,24 +635,50 @@ class NaoTeleop: 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}") + 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() - 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. + # TYPE_HELLO: nothing to do with it yet, just skip. except Exception as e: - print(f"media relay error: {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() @@ -533,11 +697,12 @@ class NaoTeleop: 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). + # 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)") + 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) @@ -970,6 +1135,8 @@ 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 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): @@ -998,8 +1165,11 @@ if __name__ == "__main__": "(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 of the on-robot media relay (nao_video_server.py), " - "e.g. 127.0.0.1:8000 when tunneled through the phone. When set, " + 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 " @@ -1016,6 +1186,13 @@ if __name__ == "__main__": 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() @@ -1037,4 +1214,6 @@ if __name__ == "__main__": 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() diff --git a/start.sh b/start.sh index a9e1b45..0c25bc3 100755 --- a/start.sh +++ b/start.sh @@ -1,4 +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 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 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