#!/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 # ========================================== # 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", 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.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, 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", relay_port=5555, relay_rate=16000): 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) 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, 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.display_width = int(320 * video_scale) self.display_height = int(240 * video_scale) 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): try: self.video = self.session.service("ALVideoDevice") self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25) print("āœ… Camera ready") except: print("āŒ Camera not available") self.video_client = None 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 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...") 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 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() 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 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}") 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)) 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._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_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("--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("--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, nao_ssh_user=args.nao_ssh_user, nao_ssh_password=args.nao_ssh_password, relay_port=args.relay_port, relay_rate=args.relay_rate).run()