#!/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 video connection (server->client only) and one audio connection # (bidirectional - server->client AUDIO is the robot's own mic, # client->server TALK is push-to-talk audio to play on the robot's # speaker), both length-framed: [1 byte type][4 byte length][payload]. # Replaces both the old HTTP MJPEG pull AND the old SSH `nc | aplay` # push-to-talk relay - everything audio/video now goes through this # one pair of connections. # ========================================== TYPE_HELLO = 0x00 TYPE_VIDEO = 0x01 TYPE_AUDIO = 0x02 TYPE_TALK = 0x03 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 encode_ulaw(pcm16): """int16 numpy array -> uint8 numpy array, mu-law companded. Mirror of nao_video_server.py's encode_ulaw() - used here now to compress push-to-talk audio before it crosses the (possibly tunneled) link, the same way the robot's own mic audio already was. Must match nao_video_server.py's decode_ulaw() exactly.""" mu = 255.0 x = np.clip(pcm16.astype(np.float64) / 32768.0, -1.0, 1.0) y = np.sign(x) * np.log1p(mu * np.abs(x)) / np.log1p(mu) return (((y + 1.0) / 2.0) * 255.0).round().astype(np.uint8) 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 def _send_framed(sock, msg_type, payload): """Mirror of nao_video_server.py's _send() - used to push TYPE_TALK (push-to-talk audio) the other way, client -> robot.""" sock.sendall(struct.pack(">BI", msg_type, len(payload))) sock.sendall(payload) # ========================================== # NAO MIC STREAM # ========================================== class SoundReceiver: PREBUFFER_CHUNKS = 1 # was 3 - less silence before playback starts TARGET_QUEUE_DEPTH = 4 # once a stall lets the backlog build past # QUEUE_MAXSIZE and we start dropping, trim # it back down to this instead of just # shaving one chunk at a time - a played-back # backlog sounds like the mic sped up, and # that's more noticeable than a couple of # dropped chunks QUEUE_MAXSIZE = 12 # was 40 - bounds worst-case playback lag 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=1024) self._play_test_tone() self.queue = queue.Queue(maxsize=self.QUEUE_MAXSIZE) 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 # A stall (network hiccup, etc.) can let the backlog build up # even before the queue is literally Full - trim it back down # toward real-time here rather than only reacting once it hits # the ceiling above, so a brief stall degrades to a couple of # dropped chunks instead of several seconds of played-back lag. while self.queue.qsize() > self.TARGET_QUEUE_DEPTH: try: self.queue.get_nowait() except queue.Empty: break 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 # ========================================== # PUSH-TO-TALK (over the media relay's audio connection) # ========================================== class PushToTalk: """ Push-to-talk: streams the local mic to NAO's speaker over the same TCP audio connection the media relay already uses for the robot's own mic (see NaoTeleop._audio_relay_loop / _send_talk_chunk) - mu-law encoded, framed as TYPE_TALK, decoded and played by nao_video_server.py's TalkPlayer. This replaces the old SSH-based relay (`nc -l | aplay` over a dedicated SSH channel): no SSH connection of its own, no per-press socket handshake - start()/stop() just toggle a background thread that reads the mic and hands chunks to send_fn, which writes straight into the already-open, already-connected audio socket. """ def __init__(self, send_fn, is_ready_fn, rate=16000, chunk_frames=1024, input_device_index=None, pyaudio_instance=None): self.send_fn = send_fn self.is_ready_fn = is_ready_fn self.rate = rate self.chunk_frames = chunk_frames self.input_device_index = input_device_index # 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) self.active = False # "idle" | "live" - read by the HUD to show status self.state = "idle" self._stream = None self._thread = None def start(self): if self.active: return if not HAS_AUDIO: print("šŸŽ™ļø Live mic needs pyaudio installed") return if not self.is_ready_fn(): print("šŸŽ™ļø Talk relay: audio connection not ready yet") return # If a previous relay thread is still tearing itself down, wait for # it to finish before reusing self._stream for a new one. if self._thread and self._thread.is_alive(): self._thread.join(timeout=2.0) self.active = True self.state = "live" 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 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() closes it, from its own thread. """ 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 _run(self): try: 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=self.chunk_frames) print("šŸŽ™ļø Talk relay: live") while self.active: try: chunk = self._stream.read(self.chunk_frames, exception_on_overflow=False) pcm = np.frombuffer(chunk, dtype=np.int16) self.send_fn(encode_ulaw(pcm).tobytes()) except Exception: break except Exception as e: print(f"šŸŽ™ļø Talk relay error: {e}") finally: self.active = False self.state = "idle" if self._stream: try: self._stream.stop_stream() self._stream.close() except: pass self._stream = None print("šŸŽ™ļø Talk relay: stopped") def close(self): """No persistent connection of its own to tear down anymore - kept as an alias for stop() so shutdown code doesn't need to know which push-to-talk implementation it's holding.""" self.stop() # ========================================== # 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: HEAD_RATE = 1.4 # rad/sec - how fast a held IJKL key sweeps the head target angle def __init__(self, session, nao_ip, audio_device_index=None, mic_gain=8.0, video_scale=2.0, nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_rate=16000, 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 # only used for volume get/set now - video # and both mic directions are relay-only self.audio_device_index = audio_device_index self.mic_gain = mic_gain # One shared PyAudio context for the whole app - SoundReceiver (mic # playback) and PushToTalk (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 # The audio relay connection is bidirectional (see # _audio_relay_loop) - this is the socket PushToTalk writes # TYPE_TALK chunks into, guarded by a lock since the relay loop # (on its own thread) can tear it down and reconnect at any time. self._audio_sock = None self._audio_sock_lock = threading.Lock() self.mic_relay = PushToTalk(send_fn=self._send_talk_chunk, is_ready_fn=self._audio_relay_ready, 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._last_head_update = time.time() 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.media_relay = media_relay 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 / 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 # Deploy+launch nao_video_server.py over SSH now (before _init_video # tries to connect to it) instead of requiring it to already be # running manually. Everything - video, robot mic, push-to-talk - # goes through this relay; there's no direct-qi fallback anymore. self.video_server_launcher = None if 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() 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(self): print(f"āœ… Camera via on-robot media relay: {self.media_relay}") threading.Thread(target=self._video_relay_loop, daemon=True).start() def _video_relay_loop(self): """Runs on its own thread for the app's lifetime, speaking the lean length-framed TCP protocol from nao_video_server.py. The render loop never touches the socket directly; it only ever reads whatever self._latest_frame was last successfully set to, so a stalled/reconnecting link can no longer freeze keyboard input or movement. Video-only connection - a slow video send could otherwise 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). This connection is bidirectional: this loop reads TYPE_AUDIO (robot's mic) off it, and _send_talk_chunk (called from the PushToTalk thread) writes TYPE_TALK (push-to-talk audio) onto the same socket - TCP is full-duplex, so neither direction blocks the other. self._audio_sock is published here (and cleared on disconnect) so _send_talk_chunk always has whatever socket is currently live, or nothing to send to while reconnecting.""" 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) with self._audio_sock_lock: self._audio_sock = sock 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: with self._audio_sock_lock: if self._audio_sock is sock: self._audio_sock = None if sock: try: sock.close() except: pass if not self._video_stop_event.is_set(): time.sleep(0.5) def _audio_relay_ready(self): """Whether the audio connection is currently up - PushToTalk checks this before starting a press so it can fail fast with a clear message instead of silently sending into the void.""" with self._audio_sock_lock: return self._audio_sock is not None def _send_talk_chunk(self, payload): """Called from the PushToTalk thread with one already mu-law- encoded chunk. Grabs whatever socket _audio_relay_loop currently has published and writes straight into it - if the link drops mid-send this just raises/gets swallowed and the chunk is lost, which is fine for live voice (the next chunk is a beat away).""" with self._audio_sock_lock: sock = self._audio_sock if not sock: return try: _send_framed(sock, TYPE_TALK, payload) except Exception: pass # _audio_relay_loop will notice the failure and reconnect 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): try: self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain, pyaudio_instance=self._pyaudio) 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() 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() now = time.time() dt = now - self._last_head_update self._last_head_update = now step = self.HEAD_RATE * dt changed = False if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - step, -0.5); changed = True if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + step, 0.5); changed = True if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + step, 2.0); changed = True if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - step, -2.0); changed = True if changed: self._safe("Head move", lambda: self.motion.setAngles( ["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.18)) 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") 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 getattr(self, "sound_receiver", None): self._safe("sound receiver close", self.sound_receiver.close) if getattr(self, "_pyaudio", None): self._safe("pyaudio terminate", self._pyaudio.terminate) self._safe("rest", self.motion.rest) pygame.quit() if __name__ == "__main__": 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-gain", type=float, default=8.0) parser.add_argument("--video-scale", type=float, default=2.0) 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. To change the mic channel, set MIC_CHANNEL in " "nao_video_server.py, 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-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_gain=args.mic_gain, video_scale=args.video_scale, 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_rate=args.relay_rate, deploy_video_server=not args.no_deploy_video_server, video_server_path=args.video_server_path).run()