#!/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: PREBUFFER_CHUNKS = 3 SILENCE_CHUNK = b"\x00" * 4096 def __init__(self, output_device_index=None, mic_gain=8.0): 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 # NEW: Amplification (higher = louder) self.mic_gain = float(mic_gain) if HAS_AUDIO: self.p = pyaudio.PyAudio() 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}") 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()) print("šŸ”” Played a test beep. If you didn't hear it, fix laptop audio first.") except Exception as e: print(f"āš ļø Test tone failed: {e}") def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): if not HAS_AUDIO: return self.chunks_received += 1 try: self.queue.put_nowait(bytes(buffer)) except queue.Full: try: self.queue.get_nowait() self.queue.put_nowait(bytes(buffer)) except queue.Empty: 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 Exception: pass now = time.time() if now - self._last_status_print > 10: self._last_status_print = now print(f"šŸŽ¤ audio: {self.chunks_received} chunks, {self.underrun_count} underruns, " f"peak {self._peak_since_print}/32767 | gain={self.mic_gain}x") self._peak_since_print = 0 def _write_chunk(self, raw_bytes): try: # === AMPLIFICATION === samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32) if samples.size: amplified = samples * self.mic_gain amplified = np.clip(amplified, -32768, 32767) # prevent clipping distortion boosted_bytes = amplified.astype(np.int16).tobytes() peak = int(np.abs(amplified).max()) if peak > self._peak_since_print: self._peak_since_print = peak self.stream.write(boosted_bytes) else: self.stream.write(raw_bytes) 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, mic_gain=8.0): 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 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 # NEW 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.typing_mode = False self.chat_message = "" self._init_audio_device() self._init_video_maxfps() if HAS_AUDIO: self._init_mic_stream() 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: {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, mic_gain=self.mic_gain ) self.session.registerService(self.audio_service_name, self.sound_receiver) time.sleep(0.5) self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0) self.audio_device.subscribe(self.audio_service_name) print(f"āœ… Live Audio Stream ready (mic gain = {self.mic_gain}x)") 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) 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: if self.typing_mode: if event.key == pygame.K_RETURN: if self.chat_message.strip(): print(f"šŸ—£ļø NAO saying: {self.chat_message}") threading.Thread(target=self.tts.say, args=(self.chat_message,)).start() self.chat_message = "" self.typing_mode = False 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 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) 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() 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)) 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) print("šŸ›‘ Shutting down...") self.motion.stopMove() 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() 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) parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left") parser.add_argument("--mic-gain", type=float, default=8.0, help="Microphone amplification (try 6-12). Higher = louder.") 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], mic_gain=args.mic_gain).run()