#!/usr/bin/env python # -*- encoding: UTF-8 -*- import qi import argparse import sys import time import threading 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: def __init__(self): if HAS_AUDIO: self.p = pyaudio.PyAudio() # NAO front mic is usually 16000Hz, 1 channel, 16-bit self.stream = self.p.open(format=pyaudio.paInt16, channels=1, rate=16000, output=True) # ALAudioDevice strictly requires this exact method signature to send data def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): if HAS_AUDIO and self.stream: try: # Play the raw audio bytes pushed from the robot directly to your speakers self.stream.write(bytes(buffer)) except Exception as e: pass # ========================================== # MAIN TELEOP CLASS # ========================================== class NaoTeleop: def __init__(self, session): 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.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 self.battery_level = 100 self.last_batt_check = 0 # Chatbox variables self.typing_mode = False self.chat_message = "" # Initialize Video self._init_video_maxfps() # Initialize Audio if HAS_AUDIO: self._init_audio() # 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() 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_audio(self): try: self.audio_device = self.session.service("ALAudioDevice") # Register our local class as a service on the robot's network self.sound_receiver = SoundReceiver() self.session.registerService(self.audio_service_name, self.sound_receiver) # Configure: 16000Hz, Channel 3 (Front Mic), 0 (Interleaved) self.audio_device.setClientPreferences(self.audio_service_name, 16000, 3, 0) self.audio_device.subscribe(self.audio_service_name) print("✅ Live Audio Stream ready (Check your speakers!)") except Exception as e: print(f"❌ Audio init failed: {e}") self.audio_device = None 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 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() mode = "[TYPING] " if self.typing_mode else "" pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}%") except: pass 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(" 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 # 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 # 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__": parser = argparse.ArgumentParser() parser.add_argument("--ip", type=str, default="127.0.0.1") parser.add_argument("--port", type=int, default=9559) 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).run()