diff --git a/naowalk.py b/naowalk.py index 09f7aaf..386e7d7 100644 --- a/naowalk.py +++ b/naowalk.py @@ -10,18 +10,18 @@ import queue import cv2 import numpy as np import pygame +import subprocess -# --- Audio Library --- +# --- Audio for mic --- try: import pyaudio HAS_AUDIO = True except ImportError: HAS_AUDIO = False - print("āš ļø PyAudio not installed. Run 'pip install pyaudio' to hear the robot.") - + print("āš ļø PyAudio not installed.") # ========================================== -# AUDIO RECEIVER +# NAO MIC STREAM TO LAPTOP # ========================================== class SoundReceiver: PREBUFFER_CHUNKS = 3 @@ -46,9 +46,9 @@ class SoundReceiver: 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']}") - except Exception as e: - print(f"āš ļø Could not resolve output device: {e}") + 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, @@ -64,9 +64,7 @@ class SoundReceiver: 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("šŸ”” Test beep played.") - except Exception as e: - print(f"āš ļø Test tone failed: {e}") + except: pass def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): if not HAS_AUDIO: return @@ -77,8 +75,7 @@ class SoundReceiver: try: self.queue.get_nowait() self.queue.put_nowait(bytes(buffer)) - except queue.Empty: - pass + except: pass def _playback_loop(self): primed = [] @@ -98,14 +95,12 @@ class SoundReceiver: self.underrun_count += 1 try: self.stream.write(self.SILENCE_CHUNK) - except Exception: - pass + except: 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") + 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): @@ -119,10 +114,7 @@ class SoundReceiver: self.stream.write(amplified.astype(np.int16).tobytes()) 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}") + except: pass def close(self): self.running = False @@ -137,7 +129,86 @@ class SoundReceiver: # ========================================== -# MAIN TELEOP CLASS +# MUSIC PLAYER ON NAO +# ========================================== +class NaoMusicPlayer: + def __init__(self, session): + self.session = session + self.player = session.service("ALAudioPlayer") + self.music_dir = "/home/nao/music" + try: + self.session.service("ALFileManager").createFolder(self.music_dir) + except: + pass + self.current_file = None + self.is_playing = False + + def search_and_play(self, query): + print(f"šŸ” Searching: {query}") + try: + result = subprocess.run(['ytfzf', '-m', '-L', query], capture_output=True, text=True, timeout=15) + url = result.stdout.strip().split('\n')[0] if result.stdout.strip() else None + if not url: + import yt_dlp + with yt_dlp.YoutubeDL({'quiet': True, 'extract_flat': True}) as ydl: + info = ydl.extract_info(f"ytsearch1:{query}", download=False) + url = info['entries'][0]['url'] + except Exception as e: + print(f"Search failed: {e}") + return False + + self.play_on_nao(url, query) + return True + + def play_on_nao(self, url, title=""): + self.stop() + try: + filename = f"{int(time.time())}.mp3" + local_path = f"/tmp/{filename}" + remote_path = f"{self.music_dir}/{filename}" + + import yt_dlp + ydl_opts = {'format': 'bestaudio/best', 'outtmpl': local_path, 'quiet': True, + 'postprocessors': [{'key': 'FFmpegExtractAudio', 'preferredcodec': 'mp3'}]} + with yt_dlp.YoutubeDL(ydl_opts) as ydl: + ydl.download([url]) + + file_manager = self.session.service("ALFileManager") + file_manager.put(local_path, remote_path) + + self.current_file = remote_path + self.player.playFile(remote_path) + self.is_playing = True + print(f"ā™Ŗ Playing on NAO: {title}") + except Exception as e: + print(f"Playback error: {e}") + + def pause(self): + if self.is_playing: + self.player.pause() + self.is_playing = False + print("āø Paused") + else: + self.player.play(self.current_file) + self.is_playing = True + print("ā–¶ Resumed") + + def stop(self): + self.player.stopAll() + self.is_playing = False + print("ā¹ Stopped") + + def seek(self, seconds): + try: + pos = self.player.getCurrentPosition() / 1000.0 + self.player.goTo(pos + seconds) + print(f"Seeked {seconds:+.0f}s") + except: + print("Seek unavailable") + + +# ========================================== +# MAIN TELEOP # ========================================== class NaoTeleop: def __init__(self, session, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0): @@ -145,15 +216,13 @@ class NaoTeleop: 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.music = NaoMusicPlayer(session) # Music on NAO + self.audio_device = None self.audio_service_name = "SoundReceiver" self.audio_device_index = audio_device_index @@ -181,7 +250,7 @@ class NaoTeleop: pygame.init() self.screen = pygame.display.set_mode((self.display_width, self.display_height)) - pygame.display.set_caption("NAO Teleop | Battery: Checking...") + pygame.display.set_caption("NAO Teleop + Music on Robot") self.font = pygame.font.SysFont(None, 24) self.clock = pygame.time.Clock() self.update_title() @@ -192,7 +261,7 @@ class NaoTeleop: self.volume = self.audio_device.getOutputVolume() print(f"šŸ”Š NAO speaker volume: {self.volume}%") except Exception as e: - print(f"āš ļø Could not reach ALAudioDevice: {e}") + print(f"āš ļø ALAudioDevice error: {e}") self.audio_device = None def _init_video_maxfps(self): @@ -202,23 +271,22 @@ class NaoTeleop: print("āœ… Camera ready") except: print("āŒ Camera not available") + self.video_client = None def _init_mic_stream(self): - if not self.audio_device: - print("āŒ Can't start mic listening") - return + if not self.audio_device: return try: - self.sound_receiver = SoundReceiver(output_device_index=self.audio_device_index, mic_gain=self.mic_gain) + self.sound_receiver = SoundReceiver(self.audio_device_index, 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 (gain = {self.mic_gain}x)") + print(f"āœ… Mic stream ready (gain={self.mic_gain}x)") except Exception as e: - print(f"āŒ Audio init failed: {e}") + print(f"āŒ Mic init failed: {e}") def get_image(self): - if not self.video or not self.video_client: + if not hasattr(self, 'video') or not self.video_client: return None try: image = self.video.getImageRemote(self.video_client) @@ -231,32 +299,18 @@ class NaoTeleop: pass return None - def enhance_frame(self, frame): - """Simple fast upscale""" - if frame is None: - return None - return cv2.resize(frame, (self.display_width, self.display_height), - interpolation=cv2.INTER_CUBIC) - 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 + 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_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] - self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3) + neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] + self.motion.angleInterpolationWithSpeed(names, neutral, 0.3) self.motion.setStiffnesses("RArm", 0.6) print("āœ… Wave completed") except Exception as e: @@ -267,12 +321,10 @@ class NaoTeleop: 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) @@ -284,16 +336,15 @@ class NaoTeleop: 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}%") + pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | 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}%") + except: pass + print(f"šŸ”Š NAO volume: {self.volume}%") self.update_title() def check_battery(self): @@ -302,8 +353,7 @@ class NaoTeleop: if now - self.last_batt_check > 10: try: self.battery_level = self.battery.getBatteryCharge() - except: - pass + except: pass self.update_title() self.last_batt_check = now @@ -312,18 +362,20 @@ class NaoTeleop: 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(" SPACE = Reset head to neutral") - print(" 1 = Wave") - print(" T = Type to Speak (TTS)") - print(" - / = = NAO speaker volume") - print(" 0 = Mute NAO speaker") - print(" ESC = Quit") + print(" WASD/Arrows = Walk") + print(" I J K L = Head") + print(" SPACE = Reset head") + print(" 1 = Wave") + print(" M = Music search (plays on NAO)") + print(" P = Pause/Resume music") + print(" S = Stop music") + print(" ā† ā†’ = Seek Ā±10s") + print(" T = TTS") + print(" ESC = Quit") while self.running: self.check_battery() @@ -335,15 +387,12 @@ class NaoTeleop: 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: @@ -355,36 +404,40 @@ class NaoTeleop: self.wave_gesture() elif event.key == pygame.K_SPACE: self.reset_head() + elif event.key == pygame.K_m: + query = input("Search music to play on NAO: ").strip() + if query: + self.music.search_and_play(query) + elif event.key == pygame.K_p: + self.music.pause() + elif event.key == pygame.K_s: + self.music.stop() + elif event.key == pygame.K_LEFT: + self.music.seek(-10) + elif event.key == pygame.K_RIGHT: + self.music.seek(10) 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_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: - enhanced = self.enhance_frame(frame) - rgb = cv2.cvtColor(enhanced, cv2.COLOR_BGR2RGB) + 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)) else: @@ -393,22 +446,23 @@ class NaoTeleop: 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)) + 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) print("šŸ›‘ Shutting down...") self.motion.stopMove() + self.music.stop() 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: + if hasattr(self, 'video') and self.video_client: try: self.video.unsubscribe(self.video_client) except: pass self.motion.rest() @@ -435,7 +489,7 @@ if __name__ == "__main__": print(f"āŒ Connection failed: {e}") sys.exit(1) - NaoTeleop(session, + NaoTeleop(session, audio_device_index=args.audio_device_index, mic_channel=MIC_CHANNELS[args.mic_channel], mic_gain=args.mic_gain, diff --git a/readme.md b/readme.md new file mode 100644 index 0000000..6af966c --- /dev/null +++ b/readme.md @@ -0,0 +1,24 @@ +# NAO Walk - Teleoperation + Music Player + +A full-featured Python teleoperation tool for the NAO robot with live camera feed, head control, microphone streaming to your laptop, and **music playback directly on the robot**. + +--- + +## Features + +- **Live Camera Feed** from NAO (upscaled on PC) +- **Keyboard Teleop** (WASD / Arrow keys) +- **Head Control** (I J K L) +- **Wave Gesture** (Key `1`) +- **Live Mic Streaming** from NAO to your laptop (with adjustable gain) +- **Music Player on NAO** ā€” Search & play YouTube music directly through the robot's speakers +- **TTS Chat** ā€” Type messages for the robot to speak (`T`) +- **Battery & Volume Display** + +--- + +## Requirements + +### On your laptop: +```bash +pip install qi opencv-python numpy pygame yt-dlp