luccapirovano/naowalk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Implement HTTP compression

Browse Source
This commit is contained in:
Lucca Pirovano
2026-07-20 17:57:24 -04:00
parent 5977e6fc7d
commit 4a4625028b
9 changed files with 359 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
naowalk.py
+162 -33
View File
@@ -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()