#!/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") # ========================================== # ON-ROBOT VIDEO/AUDIO RELAY SERVER (auto-deploy) # ========================================== class VideoServerLauncher: """ SCPs nao_video_server.py to the robot's home directory over SFTP and launches it via a persistent, pty-backed SSH session - so the user never has to manually copy the file over or SSH in to start/stop it. Uses the same trick as LiveMicRelay: a pty ties the remote process group to the SSH channel, so close() (killing the channel) reliably kills the video server too instead of leaving it orphaned on the robot, still holding VIDEO_PORT/AUDIO_PORT, for next time. """ def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22, local_path=None, remote_filename="nao_video_server.py"): self.nao_ip = nao_ip self.ssh_user = ssh_user self.ssh_password = ssh_password self.ssh_port = ssh_port self.local_path = local_path or os.path.join( os.path.dirname(os.path.abspath(__file__)), remote_filename) self.remote_filename = remote_filename self.ready = False self._ssh_client = None self._channel = None self._drain_thread = None def deploy_and_start(self): if not HAS_SSH: print("šŸŽ„ Video server auto-deploy needs paramiko: pip install paramiko") return False if not os.path.isfile(self.local_path): print(f"šŸŽ„ Video server auto-deploy: can't find {self.local_path}") return False 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=10) # Best-effort: a previous session that didn't get to call # close() cleanly (laptop lost network, crashed, etc.) can # leave a stale instance squatting on the ports - clear it # before starting a fresh one. try: _, out, _ = self._ssh_client.exec_command( f"pkill -f {self.remote_filename}", timeout=5) out.channel.recv_exit_status() time.sleep(0.3) except Exception: pass sftp = self._ssh_client.open_sftp() try: remote_home = sftp.normalize(".") # robot's home dir remote_path = remote_home + "/" + self.remote_filename print(f"šŸŽ„ Copying {self.remote_filename} -> {self.nao_ip}:{remote_path}") sftp.put(self.local_path, remote_path) finally: sftp.close() self._channel = self._ssh_client.get_transport().open_session() self._channel.get_pty() remote_cmd = ( "export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages && " "export LD_LIBRARY_PATH=/opt/aldebaran/lib && " f"python2 {remote_path}" ) self._channel.exec_command(remote_cmd) self._drain_thread = threading.Thread(target=self._drain_output, daemon=True) self._drain_thread.start() time.sleep(1.0) # give it a moment to bind before the client tries to connect if self._channel.exit_status_ready(): print("šŸŽ„ Video server exited immediately - check the log lines above") self.ready = False return False self.ready = True print("šŸŽ„ Video server deployed and running on robot") return True except Exception as e: print(f"šŸŽ„ Video server auto-deploy failed: {e}") self.ready = False return False def _drain_output(self): """Keeps reading the remote process's stdout/stderr so its pty buffer never fills up and stalls the server, and so its own status prints (quality tier changes, client connects) surface locally too.""" chan = self._channel while chan and not chan.closed: try: got = False if chan.recv_ready(): data = chan.recv(4096) if data: print(f"šŸŽ„ [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}") got = True if chan.recv_stderr_ready(): data = chan.recv_stderr(4096) if data: print(f"šŸŽ„ [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}") got = True if not got: time.sleep(0.1) except Exception: break def close(self): """Kill the pty-backed channel (which kills the remote python process with it) and tear down the SSH connection. Call once at app shutdown.""" if self._channel: try: self._channel.close() except: pass self._channel = None if self._ssh_client: try: self._ssh_client.close() except: pass self._ssh_client = None self.ready = False # ========================================== # 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, deploy_video_server=True, video_server_path=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 # If we're using the on-robot media relay, deploy+launch # nao_video_server.py over SSH now (before _init_video_maxfps # tries to connect to it) instead of requiring it to already be # running manually. self.video_server_launcher = None if self.media_relay and deploy_video_server: self.video_server_launcher = VideoServerLauncher( nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password, ssh_port=nao_ssh_port, local_path=video_server_path) self.video_server_launcher.deploy_and_start() 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 via on-robot media relay: {self.media_relay}") threading.Thread(target=self._video_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 _video_relay_loop(self): """Same role as _video_loop, but speaks the lean length-framed TCP protocol from nao_video_server.py instead of pulling raw frames over the qi session. Video-only connection - it used to share one socket with audio, but a slow video send could then block audio behind it, so they're split (see the audio port, media_relay port + 1, handled by _audio_relay_loop).""" 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"āœ… Video 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() # TYPE_HELLO: nothing to do with it yet, just skip. except Exception as e: print(f"video relay error: {e}") finally: if sock: try: sock.close() except: pass if not self._video_stop_event.is_set(): time.sleep(0.5) def _audio_relay_loop(self): """Mic audio's own connection - see _video_relay_loop for why this is split off rather than sharing video's socket. Connects to media_relay's host:port+1 by convention (matches AUDIO_PORT = VIDEO_PORT + 1 in nao_video_server.py).""" host, _, port_str = self.media_relay.partition(":") video_port = int(port_str) if port_str else 8000 port = video_port + 1 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"āœ… Audio 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_AUDIO and getattr(self, "sound_receiver", None): pcm = decode_ulaw(np.frombuffer(payload, dtype=np.uint8)) self.sound_receiver.feed(pcm.tobytes()) # TYPE_HELLO: nothing to do with it yet, just skip. except Exception as e: print(f"audio 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 audio relay connection). 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, own connection)") threading.Thread(target=self._audio_relay_loop, daemon=True).start() 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 getattr(self, "video_server_launcher", None): self._safe("video server stop", self.video_server_launcher.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 to reach the on-robot media relay (nao_video_server.py) " "at, e.g. 127.0.0.1:8000 when tunneled through the phone. Setting " "this also makes naowalk auto-scp nao_video_server.py to the " "robot's home dir over SSH and launch it (see --no-deploy-video-" "server to skip that and connect to one you started yourself). " "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") parser.add_argument("--no-deploy-video-server", action="store_true", help="Don't auto-scp/launch nao_video_server.py on the robot when " "--media-relay is set - use this if you're already running it " "yourself and just want naowalk to connect to it.") parser.add_argument("--video-server-path", type=str, default=None, help="Local path to nao_video_server.py to deploy (default: the " "copy sitting next to naowalk.py)") 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, deploy_video_server=not args.no_deploy_video_server, video_server_path=args.video_server_path).run()