Implement HTTP compression

nao_video_server.py

@@ -0,0 +1,188 @@
+# -*- 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 an MJPEG video stream over plain HTTP. Only
+compressed JPEG bytes ever cross the phone tunnel, instead of raw RGB
+frames.
+
+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
+
+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.
+"""
+import json
+import time
+import threading
+
+import BaseHTTPServer
+import SocketServer
+
+import cv2
+import numpy as np
+import qi
+
+# ---- tunables ----
+NAOQI_PORT = 9561          # local qi session port (confirmed working)
+HTTP_PORT = 8000           # forward this 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
+
+
+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."""
+
+    def __init__(self, session):
+        self.video = session.service("ALVideoDevice")
+        self.client = self.video.subscribeCamera(
+            "MjpegServer", 0, CAMERA_RESOLUTION, CAMERA_COLORSPACE, CAMERA_FPS)
+        self._lock = threading.Lock()
+        self._jpeg = None
+        self._stop = threading.Event()
+        self._thread = threading.Thread(target=self._loop)
+        self._thread.daemon = True
+        self._thread.start()
+
+    def _loop(self):
+        while not self._stop.is_set():
+            try:
+                image = self.video.getImageRemote(self.client)
+                if not image or len(image) < 7:
+                    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])
+                if ok:
+                    with self._lock:
+                        self._jpeg = buf.tobytes()
+            except Exception as e:
+                print("frame grab error: %s" % e)
+                time.sleep(0.2)
+
+    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 Handler(BaseHTTPServer.BaseHTTPRequestHandler):
+    def log_message(self, fmt, *args):
+        pass  # keep the robot's console quiet
+
+    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 do_POST(self):
+        length = int(self.headers.getheader("content-length", 0))
+        raw = self.rfile.read(length) if length else "{}"
+        try:
+            payload = json.loads(raw)
+        except ValueError:
+            payload = {}
+
+        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 _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):
+        try:
+            pct = self.server.battery.getBatteryCharge() if self.server.battery else -1
+        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
+
+
+class ThreadingHTTPServer(SocketServer.ThreadingMixIn, BaseHTTPServer.HTTPServer):
+    daemon_threads = True
+    allow_reuse_address = True
+
+
+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)
+
+    server = ThreadingHTTPServer(("0.0.0.0", HTTP_PORT), Handler)
+    server.grabber = FrameGrabber(session)
+    server.motion = session.service("ALMotion")
+    server.tts = session.service("ALTextToSpeech")
+    try:
+        server.battery = session.service("ALBattery")
+    except Exception:
+        server.battery = None
+
+    print("serving on :%d  (/video, /move, /stop, /say, /battery, /health)" % HTTP_PORT)
+    try:
+        server.serve_forever()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        server.grabber.close()
+
+
+if __name__ == "__main__":
+    main()

naomictest.py

@@ -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.")

naomusic.py

@@ -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()

naowalk.py

@@ -33,6 +33,13 @@ 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
+
 # ==========================================
 # NAO MIC STREAM
 # ==========================================
@@ -163,11 +170,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 +207,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
@@ -311,7 +319,8 @@ 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", relay_port=5555, relay_rate=16000):
+                 nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_port=5555, relay_rate=16000,
+                 video_fps=25, video_http_url=None):
         self.session = session
         self.nao_ip = nao_ip
         self.motion = session.service("ALMotion")
@@ -322,7 +331,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 +347,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 +366,21 @@ class NaoTeleop:
         self.music_search_pending = False
 
         self.video_scale = video_scale
+        self.video_fps = video_fps
+        self.video_http_url = video_http_url
         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
+
         self._init_audio_device()
         self._init_video_maxfps()
         if HAS_AUDIO:
@@ -388,14 +410,91 @@ 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
+            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()
+            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_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"
+        while not self._video_stop_event.is_set():
+            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)
+                        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()
+            except Exception as e:
+                print(f"video relay error: {e}")
+                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:
@@ -409,20 +508,6 @@ class NaoTeleop:
         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 +637,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 +681,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 +812,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 +825,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,6 +921,10 @@ 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):
@@ -821,7 +934,7 @@ class NaoTeleop:
                 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,10 +952,23 @@ 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 --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.")
     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,
@@ -862,7 +988,10 @@ 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,
+              video_http_url=args.video_http_url,
               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()

start.sh

@@ -1,3 +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 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

startSSHTunnel.sh

@@ -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