#!/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 # --- NEW: Audio Library --- try: import pyaudio HAS_AUDIO = True except ImportError: HAS_AUDIO = False print("āš ļø PyAudio not installed. Run 'pip install pyaudio' to hear the robot.") # ========================================== # AUDIO RECEIVER SERVICE (Runs in background) # ========================================== class SoundReceiver: # How many chunks to buffer before we start playback. This absorbs Wi-Fi # jitter so a late/slow packet doesn't starve ALSA the instant it lands. PREBUFFER_CHUNKS = 3 # Silence written to the output stream whenever the queue runs dry, so we # feed ALSA continuously instead of letting it hard-underrun (which is # what produces clicking/silence). SILENCE_CHUNK = b"\x00" * 4096 def __init__(self, output_device_index=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 if HAS_AUDIO: self.p = pyaudio.PyAudio() # Print every playback-capable device so you can tell whether # PortAudio's "default" is actually your laptop speakers (on # Linux it very often picks an HDMI or dummy device instead). 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 output device: [{chosen['index']}] {chosen['name']}" f"{' (forced via --audio-device-index)' if output_device_index is not None else ' (default)'}") except Exception as e: print(f"āš ļø Could not resolve output device: {e}") # NAO front mic is usually 16000Hz, 1 channel, 16-bit. 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() # The queue decouples the NAOqi network thread (which calls # processRemote) from the actual blocking audio write. Without # this, a slow/blocking stream.write() call inside processRemote # stalls NAOqi's callback thread, which delays the next packet, # which starves the speaker further - a feedback loop that shows # up as underruns and dropouts. 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): # Plays a short beep straight to the output device, independent of # any data from the robot. If you don't hear this, the problem is # your laptop's audio routing/volume, not the robot or the network - # troubleshoot with alsamixer / your system's sound settings first. 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()) print("šŸ”” Played a test beep. If you didn't hear it, this is a laptop " "audio output/device problem, not a robot/network problem.") except Exception as e: print(f"āš ļø Test tone failed to play: {e}") # ALAudioDevice strictly requires this exact method signature to send data. # Keep this method as fast as possible - it runs on NAOqi's network thread. def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): if not HAS_AUDIO: return self.chunks_received += 1 try: self.queue.put_nowait(bytes(buffer)) except queue.Full: # We're falling behind - drop the oldest chunk rather than # blocking the NAOqi thread (blocking here is what causes # cascading underruns). try: self.queue.get_nowait() self.queue.put_nowait(bytes(buffer)) except queue.Empty: pass def _playback_loop(self): # Wait for a small pile of chunks before we start playing, so the # very first sounds you hear aren't immediately starved. 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: # Nothing arrived in time - feed silence instead of letting # the ALSA buffer run completely dry. self.underrun_count += 1 try: self.stream.write(self.SILENCE_CHUNK) except Exception: pass now = time.time() if now - self._last_status_print > 10: self._last_status_print = now print(f"šŸŽ¤ audio: {self.chunks_received} chunks received, " f"{self.underrun_count} underruns, {self._write_errors} write errors, " f"queue depth {self.queue.qsize()}, peak level {self._peak_since_print}/32767") self._peak_since_print = 0 def _write_chunk(self, raw_bytes): try: self.stream.write(raw_bytes) samples = np.frombuffer(raw_bytes, dtype=np.int16) if samples.size: peak = int(np.abs(samples).max()) if peak > self._peak_since_print: self._peak_since_print = peak except Exception as e: self._write_errors += 1 if self._write_errors <= 3: print(f"āš ļø audio write error: {e}") def close(self): self.running = False if HAS_AUDIO: try: self.playback_thread.join(timeout=1.0) except Exception: pass try: self.stream.stop_stream() self.stream.close() self.p.terminate() except Exception: pass # ========================================== # MAIN TELEOP CLASS # ========================================== class NaoTeleop: def __init__(self, session, audio_device_index=None, mic_channel=1): self.session = session 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.video = None self.video_client = None # Audio setup variables self.audio_device = None self.audio_service_name = "SoundReceiver" self.audio_device_index = audio_device_index # which laptop output device to play the mic feed on self.mic_channel = mic_channel # which NAO mic to stream: LEFTCHANNEL(1)/RIGHTCHANNEL(2)/FRONTCHANNEL(3)/REARCHANNEL(4) self.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 self.battery_level = 100 self.last_batt_check = 0 # Volume of the NAO's own onboard speakers (0-100), controlled via # ALAudioDevice.setOutputVolume. This is separate from - and has # nothing to do with - the laptop playback of the robot's mic feed. self.volume = 50 # Chatbox variables self.typing_mode = False self.chat_message = "" # Get ALAudioDevice up front - this is needed for speaker volume # control regardless of whether PyAudio/mic-listening is available. self._init_audio_device() # Initialize Video self._init_video_maxfps() # Initialize mic listening (PyAudio -> laptop speakers) if HAS_AUDIO: self._init_mic_stream() # Initialize Pygame pygame.init() self.screen = pygame.display.set_mode((320, 240)) pygame.display.set_caption("NAO Teleop | Battery: Checking...") self.font = pygame.font.SysFont(None, 24) self.clock = pygame.time.Clock() self.update_title() 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"āš ļø Could not reach ALAudioDevice for speaker volume control: {e}") self.audio_device = None def _init_video_maxfps(self): try: self.video = self.session.service("ALVideoDevice") self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25) print("āœ… Camera ready") except: print("āŒ Camera not available") def _init_mic_stream(self): if not self.audio_device: print("āŒ Can't start mic listening - ALAudioDevice unavailable") return try: self.sound_receiver = SoundReceiver(output_device_index=self.audio_device_index) # 1. Register the local service self.session.registerService(self.audio_service_name, self.sound_receiver) # 2. Add a tiny delay to let the network handshake complete time.sleep(0.5) # 3. Which physical mic to stream. FRONTCHANNEL(3) was the # original default, but on this robot the front and rear mics # test dead (flat energy readings even up close) while left # and right respond normally - see nao_audio_diagnostic.py. # Defaulting to LEFTCHANNEL(1) accordingly; override with # --mic-channel if you want to compare. self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0) self.audio_device.subscribe(self.audio_service_name) print("āœ… Live Audio Stream ready") except Exception as e: print(f"āŒ Audio init failed: {e}") def get_image(self): if not self.video or not self.video_client: return None try: image = self.video.getImageRemote(self.video_client) if image and len(image) >= 7: w, h = image[0], image[1] arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3)) frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST) except: pass return None def wave_gesture(self): if self.typing_mode: return print("šŸ¤š Waving hello...") try: self.motion.setStiffnesses("RArm", 1.0) names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] angles = [ [-1.5, -1.5, -1.5, -1.5, -1.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, 1.0, 1.0, 1.0, 1.0] ] times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 6 self.motion.angleInterpolation(names, angles, times, True) neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3) self.motion.setStiffnesses("RArm", 0.6) print("āœ… Wave completed") except Exception as e: print(f"Wave error: {e}") def handle_head_movement(self): if self.typing_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.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3) def update_title(self): mode = "[TYPING] " if self.typing_mode else "" pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | NAO Vol: {self.volume}%") def change_volume(self, delta): self.volume = int(min(100, max(0, self.volume + delta))) if self.audio_device: try: self.audio_device.setOutputVolume(self.volume) except Exception as e: print(f"āš ļø Failed to set NAO speaker volume: {e}") print(f"šŸ”Š NAO speaker volume: {self.volume}%") self.update_title() def check_battery(self): if not self.battery: return now = time.time() if now - self.last_batt_check > 10: try: self.battery_level = self.battery.getBatteryCharge() except: pass self.update_title() self.last_batt_check = now def run(self): self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) # Settle delay for carpet time.sleep(1.0) self.motion.setMoveArmsEnabled(True, True) carpet_config = [ ["StepHeight", 0.028], ["MaxStepFrequency", 0.6], ["TorsoWy", 0.05] ] print("\nšŸŽ® Controls:") print(" WASD / Arrows = Walk") print(" I J K L = Head") print(" 1 = Wave") print(" T = Type to Speak (TTS)") print(" - / = = NAO speaker volume down / up") print(" 0 = Mute NAO speaker") print(" ESC = Quit") while self.running: self.check_battery() for event in pygame.event.get(): if event.type == pygame.QUIT: self.running = False elif event.type == pygame.KEYDOWN: # --- CHAT / TTS MODE --- if self.typing_mode: if event.key == pygame.K_RETURN: if self.chat_message.strip(): print(f"šŸ—£ļø NAO saying: {self.chat_message}") # Fire TTS in a background thread so the camera doesn't freeze threading.Thread(target=self.tts.say, args=(self.chat_message,)).start() self.chat_message = "" self.typing_mode = False # Force update title bar self.last_batt_check = 0 elif event.key == pygame.K_ESCAPE: self.chat_message = "" self.typing_mode = False self.last_batt_check = 0 elif event.key == pygame.K_BACKSPACE: self.chat_message = self.chat_message[:-1] else: self.chat_message += event.unicode # --- NORMAL DRIVING MODE --- else: if event.key == pygame.K_ESCAPE: self.running = False elif event.key == pygame.K_1: self.wave_gesture() elif event.key == pygame.K_t: self.typing_mode = True self.motion.stopMove() self.last_batt_check = 0 elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS): self.change_volume(-5) elif event.key in (pygame.K_EQUALS, pygame.K_PLUS, pygame.K_KP_PLUS): self.change_volume(5) elif event.key == pygame.K_0: self.change_volume(-self.volume) # Walking (Only if not typing) if not self.typing_mode: keys = pygame.key.get_pressed() x = y = theta = 0.0 if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.6 if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.6 if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.5 if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.5 if abs(x) > 0.1 or abs(theta) > 0.1: self.motion.moveToward(x, y, theta, carpet_config) else: self.motion.stopMove() self.handle_head_movement() # Camera Render frame = self.get_image() if frame is not None: rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1)) self.screen.blit(surf, (0, 0)) else: self.screen.fill((10, 10, 30)) # Chatbox Render if self.typing_mode: s = pygame.Surface((320, 40)) s.set_alpha(180) s.fill((0, 0, 0)) self.screen.blit(s, (0, 200)) text_surface = self.font.render(f"Say: {self.chat_message}", True, (255, 255, 255)) self.screen.blit(text_surface, (10, 210)) pygame.display.flip() self.clock.tick(0) # Shutdown sequence print("šŸ›‘ Shutting down...") self.motion.stopMove() # Clean up Audio if self.audio_device: try: self.audio_device.unsubscribe(self.audio_service_name) except: pass if getattr(self, "sound_receiver", None): self.sound_receiver.close() # Clean up Video if self.video and self.video_client: try: self.video.unsubscribe(self.video_client) except: pass 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, help="Force a specific PyAudio output device index for the robot's " "mic feed (see the 'šŸ”ˆ Available output devices' list printed " "at startup) if the system default isn't your speakers.") parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left", help="Which NAO mic to stream. Defaults to 'left' since " "nao_audio_diagnostic.py confirmed front/rear are dead on " "this unit while left/right respond normally.") 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, audio_device_index=args.audio_device_index, mic_channel=MIC_CHANNELS[args.mic_channel]).run()