#!/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 struct 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 # ========================================== # MEDIA RELAY WIRE PROTOCOL (see nao_video_server.py) # One TCP connection, length-framed: [1 byte type][4 byte length][payload]. # Replaces the old HTTP MJPEG pull - no multipart parsing, and now # carries mu-law-compressed mic audio too, not just video. # ========================================== TYPE_HELLO = 0x00 TYPE_VIDEO = 0x01 TYPE_AUDIO = 0x02 def decode_ulaw(q): """uint8 mu-law samples -> int16 PCM. Must match encode_ulaw() in nao_video_server.py exactly, or this just produces noise.""" mu = 255.0 y = (q.astype(np.float64) / 255.0) * 2.0 - 1.0 x = np.sign(y) * (1.0 / mu) * (np.power(1.0 + mu, np.abs(y)) - 1.0) return np.clip(x * 32768.0, -32768, 32767).astype(np.int16) def _recv_exact(sock, n): """Read exactly n bytes from a TCP socket (recv() can return short reads/chunks at any point - never assume one call gives you n).""" buf = b"" while len(buf) < n: chunk = sock.recv(n - len(buf)) if not chunk: raise IOError("media relay connection closed") buf += chunk return buf # ========================================== # 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): self.feed(bytes(buffer)) def feed(self, raw_pcm_bytes): """Push one chunk of 16-bit PCM into the playback queue, dropping the oldest chunk on overflow instead of blocking. Used both by the direct qi audio callback (processRemote, above) and by the TCP media relay path (which mu-law-decodes back to PCM before calling this).""" if not HAS_AUDIO: return self.chunks_received += 1 try: self.queue.put_nowait(raw_pcm_bytes) except queue.Full: try: self.queue.get_nowait() self.queue.put_nowait(raw_pcm_bytes) 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, media_relay=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.media_relay = media_relay # True only if we actually subscribed to ALAudioDevice ourselves # (i.e. not using the relay, where nao_video_server.py owns the # mic subscription instead) - used at shutdown so we don't try # to unsubscribe from something we never subscribed to. self._qi_audio_subscribed = False 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.media_relay: self.video = None self.video_client = "relay" # truthy sentinel, unsubscribe() is a no-op for this path print(f"āœ… Camera + mic via on-robot media relay: {self.media_relay}") threading.Thread(target=self._media_relay_loop, daemon=True).start() 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 _media_relay_loop(self): """Same role as _video_loop, but speaks the lean length-framed TCP protocol from nao_video_server.py instead of pulling an HTTP MJPEG stream - and now demuxes BOTH video and mu-law- compressed mic audio off the same connection, since the mic subscription lives on the robot too now instead of shipping raw PCM through the qi RPC session.""" host, _, port_str = self.media_relay.partition(":") port = int(port_str) if port_str else 8000 while not self._video_stop_event.is_set(): sock = None try: sock = socket.create_connection((host, port), timeout=5) sock.settimeout(5.0) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) print(f"āœ… Media relay connected: {host}:{port}") while not self._video_stop_event.is_set(): msg_type, length = struct.unpack(">BI", _recv_exact(sock, 5)) payload = _recv_exact(sock, length) if length else b"" if msg_type == TYPE_VIDEO: arr = np.frombuffer(payload, dtype=np.uint8) frame = cv2.imdecode(arr, cv2.IMREAD_COLOR) if frame is not None: self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST) self._latest_frame_ts = time.time() elif msg_type == TYPE_AUDIO: if getattr(self, "sound_receiver", None): pcm = decode_ulaw(np.frombuffer(payload, dtype=np.uint8)) self.sound_receiver.feed(pcm.tobytes()) # TYPE_HELLO carries {"rate", "channels"} for future use - nothing to do with it yet. except Exception as e: print(f"media relay error: {e}") finally: if sock: try: sock.close() except: pass if not self._video_stop_event.is_set(): 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: # Playback machinery is needed either way - only the source # of audio chunks differs (qi pub/sub below, vs mu-law # chunks arriving over the media relay in _media_relay_loop). self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain, pyaudio_instance=self._pyaudio) if self.media_relay: print(f"āœ… Mic stream ready (gain={self.mic_gain}x, via media relay - compressed)") return 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) self._qi_audio_subscribed = True print(f"āœ… Mic stream ready (gain={self.mic_gain}x, uncompressed direct - use --media-relay on a tunneled link)") 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 and self._qi_audio_subscribed: 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 --media-relay is set (the server controls fps).") parser.add_argument("--media-relay", type=str, default=None, help="host:port of the on-robot media relay (nao_video_server.py), " "e.g. 127.0.0.1:8000 when tunneled through the phone. When set, " "BOTH video and mic audio come compressed over this one TCP " "connection (JPEG + mu-law) instead of raw frames/PCM over the " "qi session - use this on a tunneled/shoddy link. --mic-channel " "is ignored in this mode (set MIC_CHANNEL in nao_video_server.py " "instead, since the mic subscription lives there now).") 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, media_relay=args.media_relay, 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()