#!/usr/bin/env python # -*- encoding: UTF-8 -*- import qi import argparse import sys import time import threading import queue import cv2 import numpy as np import pygame import subprocess import os import json import re import socket # Import the new music module from naomusic import NaoMusicPlayer # --- Audio for mic --- try: import pyaudio HAS_AUDIO = True except ImportError: HAS_AUDIO = False # --- SSH for live mic relay (push-to-talk) --- try: import paramiko HAS_SSH = True 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 # ========================================== class SoundReceiver: PREBUFFER_CHUNKS = 3 SILENCE_CHUNK = b"\x00" * 4096 def __init__(self, output_device_index=None, mic_gain=8.0, pyaudio_instance=None): self.running = True self.underrun_count = 0 self.chunks_received = 0 self._write_errors = 0 self._peak_since_print = 0 self._last_status_print = 0 self.mic_gain = float(mic_gain) self.muted = False self._owns_pyaudio = False if HAS_AUDIO: self.p = pyaudio_instance if pyaudio_instance is not None else pyaudio.PyAudio() self._owns_pyaudio = pyaudio_instance is None print("šŸ”ˆ Available output devices:") for i in range(self.p.get_device_count()): info = self.p.get_device_info_by_index(i) if info.get("maxOutputChannels", 0) > 0: print(f" [{i}] {info['name']}") try: chosen = (self.p.get_device_info_by_index(output_device_index) if output_device_index is not None else self.p.get_default_output_device_info()) print(f"šŸ”ˆ Using: [{chosen['index']}] {chosen['name']}") except: pass self.stream = self.p.open(format=pyaudio.paInt16, channels=1, rate=16000, output=True, output_device_index=output_device_index, frames_per_buffer=2048) self._play_test_tone() self.queue = queue.Queue(maxsize=40) self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True) self.playback_thread.start() def _play_test_tone(self): try: duration, freq, rate = 0.3, 440.0, 16000 t = np.linspace(0, duration, int(rate * duration), False) tone = (np.sin(freq * t * 2 * np.pi) * 12000).astype(np.int16) self.stream.write(tone.tobytes()) except: pass def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): if not HAS_AUDIO: return self.chunks_received += 1 try: self.queue.put_nowait(bytes(buffer)) except queue.Full: try: self.queue.get_nowait() self.queue.put_nowait(bytes(buffer)) except: pass def _playback_loop(self): primed = [] while self.running and len(primed) < self.PREBUFFER_CHUNKS: try: primed.append(self.queue.get(timeout=1.0)) except queue.Empty: break for chunk in primed: self._write_chunk(chunk) while self.running: try: chunk = self.queue.get(timeout=0.2) self._write_chunk(chunk) except queue.Empty: self.underrun_count += 1 try: self.stream.write(self.SILENCE_CHUNK) except: pass now = time.time() if now - self._last_status_print > 10: self._last_status_print = now print(f"šŸŽ¤ mic: {self.chunks_received} chunks, {self.underrun_count} underruns, peak {self._peak_since_print}") self._peak_since_print = 0 def _write_chunk(self, raw_bytes): try: if self.muted: self.stream.write(b"\x00" * len(raw_bytes)) return samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32) if samples.size: amplified = np.clip(samples * self.mic_gain, -32768, 32767) peak = int(np.abs(amplified).max()) if peak > self._peak_since_print: self._peak_since_print = peak self.stream.write(amplified.astype(np.int16).tobytes()) else: self.stream.write(raw_bytes) except: pass def close(self): self.running = False if HAS_AUDIO: try: self.playback_thread.join(timeout=1.0) except: pass try: self.stream.stop_stream() self.stream.close() if self._owns_pyaudio: self.p.terminate() except: pass # ========================================== # LIVE MIC RELAY (push-to-talk over SSH) # ========================================== class LiveMicRelay: """ Push-to-talk: streams the local mic straight to NAO's own speaker. connect() (call once at startup, off the hot path) SSHes into the robot and launches a self-restarting `nc -l | aplay` loop that stays up for the whole session. Each start()/stop() (per push-to-talk press) then just opens/closes a plain TCP socket to that already- running listener and pipes raw mic audio into it - no SSH handshake per press. Closing the socket on stop() ends that iteration's nc, which ends aplay with it, and the remote loop immediately relaunches 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", 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 self.active = False self.connected = False # remote listener is up, ready for fast local connects # "idle" | "connecting" | "live" - read by the HUD to show status self.state = "idle" self._ssh_client = None self._ssh_channel = None self._sock = None self._stream = None self._thread = None # Reuse the app's single PyAudio context - a second independent one # opened from a background thread crashes PortAudio's PulseAudio # backend (pa_atomic_load assertion). self._p = pyaudio_instance if pyaudio_instance is not None else (pyaudio.PyAudio() if HAS_AUDIO else None) def connect(self): """One-time setup: SSH in and launch a self-restarting remote listener (`nc | aplay` in a loop) that stays up for the whole app session. Call this once at startup, in a background thread so it doesn't block. After this, start()/stop() per push-to-talk press only do a fast local socket connect - no SSH in the hot path. """ if not HAS_SSH: print("šŸŽ™ļø Live mic needs paramiko: pip install paramiko") return 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=5) self._ssh_channel = self._ssh_client.get_transport().open_session() # A pty ties the remote process group to this channel, so # closing it in close() reliably kills the loop/nc/aplay # instead of leaving an orphaned process on the robot. self._ssh_channel.get_pty() remote_cmd = ( f"while true; do nc -l -p {self.port} | " f"aplay -q -r {self.rate} -f S16_LE -c 1 -t raw -; done" ) self._ssh_channel.exec_command(remote_cmd) time.sleep(0.4) # one-time wait for the first nc to bind self.connected = True print("šŸŽ™ļø Talk relay: remote listener ready") except Exception as e: print(f"šŸŽ™ļø Talk relay: couldn't set up remote listener ({e})") self.connected = False def close(self): """Tear down the persistent SSH connection/listener. Call once at app shutdown - not per push-to-talk press.""" if self._ssh_channel: try: self._ssh_channel.close() except: pass self._ssh_channel = None if self._ssh_client: try: self._ssh_client.close() except: pass self._ssh_client = None self.connected = False def start(self): if self.active: return if not HAS_AUDIO: print("šŸŽ™ļø Live mic needs pyaudio installed") return if not self.connected: print("šŸŽ™ļø Talk relay: remote listener not ready yet") return # If a previous relay thread is still tearing itself down, wait for # it to finish before reusing self._stream/_sock for a new one. if self._thread and self._thread.is_alive(): self._thread.join(timeout=2.0) self.active = True self.state = "connecting" self._thread = threading.Thread(target=self._run, daemon=True) self._thread.start() def stop(self): """Signal the relay thread to stop and wait for it to exit. Deliberately does NOT touch self._stream/_sock here: this is called from the main/render thread on key-up, and closing a PortAudio stream from a different thread while the relay thread is mid blocking-read() on it is what caused the ALSA segfault on release. Only _run() (via _cleanup, in its own thread) closes them. """ self.active = False self.state = "idle" if self._thread and self._thread.is_alive() and threading.current_thread() is not self._thread: self._thread.join(timeout=2.0) def _cleanup(self): """Per-press teardown: only the local socket/mic stream. The SSH connection is persistent now - close() (at app shutdown) is what tears that down, not this.""" if self._sock: try: self._sock.close() except: pass self._sock = None if self._stream: try: self._stream.stop_stream() self._stream.close() except: pass self._stream = None def _run(self): try: # Remote listener is already up (from connect()) - just make a # fast local socket connection, no SSH handshake here. self._sock = socket.create_connection((self.nao_ip, self.port), timeout=5) self._stream = self._p.open(format=pyaudio.paInt16, channels=1, rate=self.rate, input=True, input_device_index=self.input_device_index, frames_per_buffer=1024) print("šŸŽ™ļø Talk relay: live") if self.active: # could've been stopped while we were still connecting self.state = "live" while self.active: try: chunk = self._stream.read(1024, exception_on_overflow=False) self._sock.sendall(chunk) except Exception: break except Exception as e: print(f"šŸŽ™ļø Talk relay error: {e}") finally: self.active = False self.state = "idle" self._cleanup() print("šŸŽ™ļø Talk relay: stopped") # ========================================== # 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, video_http_url=None): self.session = session self.nao_ip = nao_ip self.motion = session.service("ALMotion") self.posture = session.service("ALRobotPosture") self.tts = session.service("ALTextToSpeech") try: self.battery = session.service("ALBattery") except: self.battery = None self.music = NaoMusicPlayer(session, nao_ip, ssh_port=nao_ssh_port) self.music_results = [] self.selected_result = 0 self.audio_device = None self.audio_service_name = "SoundReceiver" self.audio_device_index = audio_device_index self.mic_channel = mic_channel self.mic_gain = mic_gain # One shared PyAudio context for the whole app - SoundReceiver (mic # playback) and LiveMicRelay (push-to-talk capture) both use it, # since two separate contexts crash PortAudio's PulseAudio backend. 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 self.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 self.battery_level = 100 self.last_batt_check = 0 self.volume = 50 self.walk_speed = 0.6 self.typing_mode = False self.chat_message = "" self.music_search_mode = False self.music_search_text = "" 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: self._init_mic_stream() pygame.init() self.screen = pygame.display.set_mode((self.display_width, self.display_height)) pygame.display.set_caption("NAO Teleop + Music on Robot") self.font = pygame.font.SysFont(None, 24) self.clock = pygame.time.Clock() self.update_title() def _safe(self, label, fn): try: return fn() except Exception as e: print(f"{label} error: {e}") return None def _init_audio_device(self): try: self.audio_device = self.session.service("ALAudioDevice") self.volume = self.audio_device.getOutputVolume() print(f"šŸ”Š NAO speaker volume: {self.volume}%") except Exception as e: print(f"āš ļø ALAudioDevice error: {e}") 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, 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: self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain, pyaudio_instance=self._pyaudio) 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)") except Exception as e: print(f"āŒ Mic init failed: {e}") def wave_gesture(self): if self.typing_mode: return print("šŸ¤š Waving hello...") try: self.motion.setStiffnesses("RArm", 1.0) names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] angles = [[-1.5]*5, [-0.4,-0.7,-0.4,-0.7,-0.4], [0.6,1.1,0.6,1.1,0.6], [0.8,0.3,0.8,0.3,0.8], [0.0,1.5,0.0,-1.5,0.0], [1.0]*5] times = [[0.5,1.0,1.5,2.0,2.5]] * 6 self.motion.angleInterpolation(names, angles, times, True) neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] self.motion.angleInterpolationWithSpeed(names, neutral, 0.3) # Relaxed stiffness after completion self.motion.setStiffnesses("RArm", 0.3) except Exception as e: print(f"Wave error: {e}") def cena_gesture(self): if self.typing_mode: return print("šŸ‘‹ YOU CAN'T SEE ME!") try: self.tts.say("You can't see me") self.motion.setStiffnesses("RArm", 1.0) names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] # Setup for tight, face-level sweep tight_setup = [-0.4, -0.2, 0.0, 1.2, 1.5, 1.0] self.motion.angleInterpolation(names, tight_setup, [0.5]*6, True) # The horizontal "backhand" sweep self.motion.angleInterpolation(["RElbowYaw"], [0.8], 0.4, True) self.motion.angleInterpolation(["RElbowYaw"], [-0.8], 0.6, True) self.motion.angleInterpolation(["RElbowYaw"], [0.0], 0.4, True) # Return to neutral neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] self.motion.angleInterpolationWithSpeed(names, neutral, 0.3) # Relaxed stiffness after completion self.motion.setStiffnesses("RArm", 0.3) except Exception as e: print(f"Cena gesture error: {e}") def six_seven_gesture(self): if self.typing_mode: return print("šŸ–ļø SIX... SEVEN...") try: threading.Thread(target=self.tts.say, args=("six seven",), daemon=True).start() self.motion.setStiffnesses(["RArm", "LArm"], 1.0) # Lock both arms up in front of the body, hands held out like a scale. # ElbowYaw is rotated ~90Ā° off zero here - that's what points the elbow's # bend axis so flexing tips the forearm forward/down instead of curling # it in across the chest. These joints don't move again until the return. hold_names = [ "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RWristYaw", "RHand", "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LWristYaw", "LHand" ] hold_angles = [ 0.2, -0.2, 1.4, 1.57, 0.6, 0.2, 0.2, -1.4, -1.57, 0.6 ] self.motion.angleInterpolationWithSpeed(hold_names, hold_angles, 0.25) # Only the elbows seesaw now - a small tip back and forth, one arm # rising while the other falls, like weighing something in each hand # ("six... seven..."). Small amplitude so it stays a wobble, not a fold. elbow_names = ["RElbowRoll", "LElbowRoll"] r_low, r_high = 0.6, 1.0 l_low, l_high = -0.6, -1.0 elbow_angles = [ [r_low, r_high, r_low, r_high, r_low, r_high], [l_high, l_low, l_high, l_low, l_high, l_low], ] times = [[0.35, 0.7, 1.05, 1.4, 1.75, 2.1]] * 2 self.motion.angleInterpolation(elbow_names, elbow_angles, times, True) # Return everything to neutral neutral_names = hold_names + elbow_names neutral = [1.5, -0.15, 1.2, 0.0, 0.0, 1.5, 0.15, -1.2, 0.0, 0.0, 0.5, -0.5] self.motion.angleInterpolationWithSpeed(neutral_names, neutral, 0.3) # Relaxed stiffness after completion self.motion.setStiffnesses(["RArm", "LArm"], 0.3) except Exception as e: print(f"Six seven gesture error: {e}") def handle_head_movement(self): if self.typing_mode or self.music_search_mode: return keys = pygame.key.get_pressed() speed = 0.3 changed = False if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True if changed: self._safe("Head move", lambda: self.motion.setAngles( ["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)) def reset_head(self): self.head_yaw = 0.0 self.head_pitch = 0.0 self._safe("Head reset", lambda: self.motion.setAngles(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.4)) def update_title(self): mode = "[TYPING] " if self.typing_mode else "" pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol: {self.volume}% | Speed: {self.walk_speed:.2f}") def change_volume(self, delta): self.volume = int(min(100, max(0, self.volume + delta))) if self.audio_device: self._safe("Volume set", lambda: self.audio_device.setOutputVolume(self.volume)) print(f"šŸ”Š NAO volume: {self.volume}%") self.update_title() def check_battery(self): if not self.battery: return now = time.time() if now - self.last_batt_check > 10: try: self.battery_level = self.battery.getBatteryCharge() except: pass 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) # Enable walk arms BEFORE forcing stiffness print("šŸ’Ŗ Enabling walk arms...") self.motion.setMoveArmsEnabled(True, True) # Force relaxed stiffness AFTER engine is active self.motion.setStiffnesses(["RArm", "LArm"], 0.3) print("šŸ¦¶ Initializing walk engine...") self._safe("moveInit", self.motion.moveInit) print("šŸ¦¶ Walk engine ready") 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: while self.running: try: self.check_battery() for event in pygame.event.get(): if event.type == pygame.QUIT: self.running = False elif event.type == pygame.KEYDOWN: if self.music_search_mode: if event.key == pygame.K_RETURN: if self.music_results: entry = self.music_results[self.selected_result] threading.Thread( target=self.music.play, args=(entry['url'], entry.get('title', 'Unknown')), daemon=True ).start() self.music_search_mode = False elif self.music_search_text.strip() and not self.music_search_pending: query = self.music_search_text self.music_search_pending = True def do_search(q=query): results = self.music.search(q) self.music_results = results self.selected_result = 0 self.music_search_pending = False if not results: self.music_search_mode = False threading.Thread(target=do_search, daemon=True).start() else: self.music_search_mode = False elif event.key == pygame.K_ESCAPE: self.music_search_mode = False self.music_search_text = "" self.music_results = [] elif event.key == pygame.K_UP: if self.music_results: self.selected_result = (self.selected_result - 1) % len(self.music_results[:5]) elif event.key == pygame.K_DOWN: if self.music_results: self.selected_result = (self.selected_result + 1) % len(self.music_results[:5]) elif event.key == pygame.K_BACKSPACE: self.music_search_text = self.music_search_text[:-1] self.music_results = [] elif event.key in (pygame.K_1, pygame.K_2, pygame.K_3, pygame.K_4, pygame.K_5): idx = int(event.unicode) - 1 if idx < len(self.music_results): entry = self.music_results[idx] threading.Thread( target=self.music.play, args=(entry['url'], entry.get('title', 'Unknown')), daemon=True ).start() self.music_search_mode = False elif event.unicode.isprintable(): self.music_search_text += event.unicode self.music_results = [] elif self.typing_mode: if event.key == pygame.K_RETURN: if self.chat_message.strip(): threading.Thread(target=self.tts.say, args=(self.chat_message,), daemon=True).start() self.chat_message = "" self.typing_mode = False elif event.key == pygame.K_ESCAPE: self.chat_message = "" self.typing_mode = False elif event.key == pygame.K_BACKSPACE: self.chat_message = self.chat_message[:-1] else: self.chat_message += event.unicode else: if event.key == pygame.K_ESCAPE: self.running = False elif event.key == pygame.K_1: self.wave_gesture() elif event.key == pygame.K_2: self.cena_gesture() elif event.key == pygame.K_3: self.six_seven_gesture() elif event.key == pygame.K_SPACE: self.reset_head() elif event.key == pygame.K_m: self.music_search_mode = True self.music_search_text = "" self.music_results = [] elif event.key == pygame.K_x: self.music.stop() elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS): self.change_volume(-5) elif event.key in (pygame.K_EQUALS, pygame.K_KP_PLUS): self.change_volume(5) elif event.key == pygame.K_t: self.typing_mode = True self._safe("stopMove", self.motion.stopMove) if not self.typing_mode and not self.music_search_mode: keys = pygame.key.get_pressed() if keys[pygame.K_v] and not self.ptt_active: self.ptt_active = True self.mic_relay.start() if getattr(self, "sound_receiver", None): self.sound_receiver.muted = True elif not keys[pygame.K_v] and self.ptt_active: self.ptt_active = False self.mic_relay.stop() if getattr(self, "sound_receiver", None): self.sound_receiver.muted = False speed_changed = False if keys[pygame.K_LEFTBRACKET]: self.walk_speed = max(0.1, self.walk_speed - 0.01) speed_changed = True if keys[pygame.K_RIGHTBRACKET]: self.walk_speed = min(1.0, self.walk_speed + 0.01) speed_changed = True if speed_changed: self.update_title() x = y = theta = 0.0 if keys[pygame.K_w] or keys[pygame.K_UP]: x = self.walk_speed if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -self.walk_speed 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 # 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() if frame is not None: up = cv2.resize(frame, (self.display_width, self.display_height), interpolation=cv2.INTER_CUBIC) 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)) # Music status if self.music.is_loading: text = self.font.render(f"ā™Ŗ {self.music.status}", True, (255, 200, 0)) self.screen.blit(text, (10, 10)) if self.music.progress > 0: bar_w = 220 pct = min(self.music.progress, 100) / 100.0 pygame.draw.rect(self.screen, (70, 70, 70), (10, 32, bar_w, 8)) pygame.draw.rect(self.screen, (255, 200, 0), (10, 32, int(bar_w * pct), 8)) elif self.music.load_error: text = self.font.render(f"ā™Ŗ Error: {self.music.load_error}", True, (255, 80, 80)) self.screen.blit(text, (10, 10)) elif self.music.current_title: text = self.font.render(f"ā™Ŗ {self.music.current_title[:45]}", True, (0, 255, 100)) self.screen.blit(text, (10, 10)) # Voice chat HUD (top-right) - only shown while V is held / connecting relay_state = getattr(self.mic_relay, "state", "idle") if relay_state == "connecting": label, color = "VOICE: CONNECTING...", (255, 200, 0) elif relay_state == "live": label, color = "VOICE: LIVE - speak now", (0, 255, 100) else: label, color = None, None if label: text = self.font.render(label, True, color) pad = 8 box_w, box_h = text.get_width() + pad * 2 + 18, text.get_height() + pad box_x = self.display_width - box_w - 10 box_y = 10 s = pygame.Surface((box_w, box_h)) s.set_alpha(180) s.fill((0, 0, 0)) self.screen.blit(s, (box_x, box_y)) # pulsing dot for "live" so it's easy to catch out of the corner of your eye if relay_state == "live": pulse = 0.5 + 0.5 * abs((pygame.time.get_ticks() % 1000) / 500.0 - 1.0) dot_color = tuple(int(c * pulse) for c in color) else: dot_color = color pygame.draw.circle(self.screen, dot_color, (box_x + pad + 5, box_y + box_h // 2), 5) self.screen.blit(text, (box_x + pad + 18, box_y + pad // 2)) # Music search interface if self.music_search_mode: box_h = min(self.display_height - 80, 260) s = pygame.Surface((self.display_width, box_h)) s.set_alpha(220) s.fill((0, 0, 40)) self.screen.blit(s, (0, 80)) text = self.font.render(f"Search: {self.music_search_text}", True, (255, 255, 255)) self.screen.blit(text, (20, 100)) if self.music_results: shown = self.music_results[:5] for i, entry in enumerate(shown): row_y = 140 + i * 30 if i == self.selected_result: pygame.draw.rect(self.screen, (90, 90, 0), (16, row_y - 3, self.display_width - 32, 26)) color = (255, 255, 0) if i == self.selected_result else (200, 200, 200) title = entry.get('title', 'No title')[:60] line = self.font.render(f"{i+1}. {title}", True, color) self.screen.blit(line, (20, row_y)) hint_y = 140 + len(shown) * 30 + 6 hint = self.font.render("\u2191\u2193 Select Enter Confirm 1-5 Quick pick Esc Cancel", True, (150, 150, 150)) self.screen.blit(hint, (20, hint_y)) elif self.music_search_pending: text = self.font.render("Searching...", True, (200, 200, 200)) self.screen.blit(text, (20, 140)) else: text = self.font.render("No results. Type to search again, Enter to search", True, (200, 200, 200)) self.screen.blit(text, (20, 140)) # TTS box if self.typing_mode: s = pygame.Surface((self.display_width, 40)) s.set_alpha(180) s.fill((0,0,0)) self.screen.blit(s, (0, self.display_height-40)) text = self.font.render(f"Say: {self.chat_message}", True, (255,255,255)) self.screen.blit(text, (10, self.display_height-30)) pygame.display.flip() self.clock.tick(0) except Exception as e: 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: 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 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) self._safe("rest", self.motion.rest) pygame.quit() if __name__ == "__main__": MIC_CHANNELS = {"left": 1, "right": 2, "front": 3, "rear": 4} parser = argparse.ArgumentParser() parser.add_argument("--ip", type=str, default="127.0.0.1") parser.add_argument("--port", type=int, default=9559) parser.add_argument("--audio-device-index", type=int, default=None) 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, help="Sample rate for the live mic relay - try 48000 if audio sounds sped up/slow") args = parser.parse_args() session = qi.Session() try: session.connect(f"tcp://{args.ip}:{args.port}") print(f"āœ… Connected to {args.ip}") except Exception as e: print(f"āŒ Connection failed: {e}") sys.exit(1) NaoTeleop(session, nao_ip=args.ip, audio_device_index=args.audio_device_index, 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()