From 69e41aa67cf6e3a2956ecc836a3a160cde5f74f4 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 14:16:16 -0400 Subject: [PATCH] added WIP for hearing the nao's mic through laptop and tts chat with T --- naowalk.py | 221 +++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 155 insertions(+), 66 deletions(-) diff --git a/naowalk.py b/naowalk.py index 1dde627..f4bf336 100644 --- a/naowalk.py +++ b/naowalk.py @@ -5,39 +5,87 @@ import qi import argparse import sys import time -import pygame +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") - # New: Hook into the battery service try: self.battery = session.service("ALBattery") except: self.battery = None - print("⚠️ ALBattery service not found") self.video = None self.video_client = None - self.running = True + + # 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 - - # Battery tracking 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): @@ -48,6 +96,21 @@ class NaoTeleop: 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 @@ -63,66 +126,51 @@ class NaoTeleop: return None def wave_gesture(self): + if self.typing_mode: return print("🤚 Waving hello...") try: - # Stiffen arm for accurate wave self.motion.setStiffnesses("RArm", 1.0) - - # Use all 6 joints to prevent xAssertArraySize crash names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] - angles = [ - [-1.5, -1.5, -1.5, -1.5, -1.5], # RShoulderPitch (high up) - [-0.4, -0.7, -0.4, -0.7, -0.4], # RShoulderRoll - [ 0.6, 1.1, 0.6, 1.1, 0.6], # RElbowYaw - [ 0.8, 0.3, 0.8, 0.3, 0.8], # RElbowRoll (proper bending) - [ 0.0, 1.5, 0.0, -1.5, 0.0], # RWristYaw (the wave) - [ 1.0, 1.0, 1.0, 1.0, 1.0] # RHand (keep open) + [-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) - # Safely lower arm to true neutral neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3) - - # Relax the arm's stiffness to let the motors cool and walk naturally self.motion.setStiffnesses("RArm", 0.6) - - print("✅ Proper wave completed and arm relaxed") - + 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 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): - # Only ping the robot for battery data once every 10 seconds to save FPS - if not self.battery: - return - + if not self.battery: return now = time.time() if now - self.last_batt_check > 10: try: self.battery_level = self.battery.getBatteryCharge() - pygame.display.set_caption(f"NAO Teleop | Battery: {self.battery_level}%") + 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 @@ -131,25 +179,21 @@ class NaoTeleop: self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) - # 1. SETTLE DELAY: Let his internal gyroscopes adapt to the squishy carpet + # Settle delay for carpet time.sleep(1.0) - self.motion.setMoveArmsEnabled(True, True) - # 2. CARPET GAIT CONFIGURATION - # StepHeight: Lifts the foot 2.8cm (higher than default) to clear fibers - # MaxStepFrequency: 0.6 slows the step rhythm down for better balance - # TorsoWy: 0.05 leans his torso just slightly forward for better momentum carpet_config = [ ["StepHeight", 0.028], ["MaxStepFrequency", 0.6], ["TorsoWy", 0.05] ] - print("🎮 Controls:") + 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: @@ -159,30 +203,55 @@ class NaoTeleop: if event.type == pygame.QUIT: self.running = False elif event.type == pygame.KEYDOWN: - if event.key == pygame.K_ESCAPE: - self.running = False - elif event.key == pygame.K_1: - self.wave_gesture() + + # --- 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 - keys = pygame.key.get_pressed() - x = y = theta = 0.0 - - # 3. SAFER TOP SPEED: Dropped from 0.9 (sprinting) to 0.6 (safe walking) - 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 + # 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: - # Pass the custom carpet gait to the walk engine - self.motion.moveToward(x, y, theta, carpet_config) - else: - self.motion.stopMove() + 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() + self.handle_head_movement() - # Camera + # Camera Render frame = self.get_image() if frame is not None: rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) @@ -191,18 +260,38 @@ class NaoTeleop: 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 + # Shutdown sequence print("🛑 Shutting down...") self.motion.stopMove() - self.motion.rest() + + # 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__":