From c69f8ca7d0751a675bb1541801c9cb724eac7d18 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 18:55:56 -0400 Subject: [PATCH] refined the music player, probably going to gut the pause and seek functionality since everything else about it works perfectly. now i need to fix the start of the walk cycle toppling the robot and the low battery errors being so loud and repetitive they drown out the music --- naowalk.py | 524 +++++++++++++++++++++++++++++++++++------------------ 1 file changed, 343 insertions(+), 181 deletions(-) diff --git a/naowalk.py b/naowalk.py index 1cfbf01..e314e0f 100644 --- a/naowalk.py +++ b/naowalk.py @@ -10,9 +10,10 @@ import queue import cv2 import numpy as np import pygame -import yt_dlp import subprocess import os +import json +import re # --- Audio for mic --- try: @@ -137,87 +138,168 @@ class NaoMusicPlayer: self.player = session.service("ALAudioPlayer") self.nao_ip = nao_ip self.music_dir = "/home/nao/music" + self.local_path = "/tmp/song.mp3" + self.remote_path = f"{self.music_dir}/song.mp3" try: - subprocess.run(["sshpass", "-p", "nao", "ssh", f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"], + subprocess.run(["sshpass", "-p", "nao", "ssh", f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) except: pass self.current_file = None + self.current_task_id = None self.is_playing = False + self.paused_position = 0.0 self.current_title = "" + self.is_loading = False + self.load_error = None + self.status = "" + self.progress = 0.0 def search(self, query): try: - ydl_opts = { - 'format': 'bestaudio/best', - 'quiet': True, - 'noplaylist': True, - 'extract_flat': True, - } - with yt_dlp.YoutubeDL(ydl_opts) as ydl: - info = ydl.extract_info(f"ytsearch5:{query}", download=False) - return info.get('entries', []) - except: + cmd = ["yt-dlp", f"ytsearch5:{query}", "--flat-playlist", "--dump-json", + "--no-warnings", "--quiet"] + result = subprocess.run(cmd, capture_output=True, text=True, timeout=30) + entries = [] + for line in result.stdout.strip().splitlines(): + if not line.strip(): + continue + try: + data = json.loads(line) + except json.JSONDecodeError: + continue + vid = data.get("id") + url = data.get("url") + if not url or not url.startswith("http"): + url = f"https://www.youtube.com/watch?v={vid}" if vid else None + if url: + entries.append({"url": url, "title": data.get("title", "Unknown")}) + return entries + except Exception as e: + print(f"Search error: {e}") return [] + _DL_PROGRESS_RE = re.compile(r'\[download\]\s+([\d.]+)%') + def play(self, url, title): self.stop() self.current_title = title + self.is_loading = True + self.load_error = None + self.status = "Starting download..." + self.progress = 0.0 try: - ydl_opts = { - 'format': 'bestaudio/best', - 'outtmpl': '/tmp/%(id)s.%(ext)s', - 'quiet': True, - 'noplaylist': True, - 'extractaudio': True, - 'postprocessors': [{'key': 'FFmpegExtractAudio', 'preferredcodec': 'mp3'}], - 'ignoreerrors': True, - 'no_warnings': True, - } - with yt_dlp.YoutubeDL(ydl_opts) as ydl: - info = ydl.extract_info(url, download=True) - if not info: - print("Download failed") - return False - downloaded = ydl.prepare_filename(info) + if os.path.exists(self.local_path): + os.remove(self.local_path) + except: pass - remote_path = f"{self.music_dir}/{int(time.time())}.mp3" - result = subprocess.run(["sshpass", "-p", "nao", "scp", downloaded, f"nao@{self.nao_ip}:{remote_path}"], - capture_output=True, text=True) - if result.returncode != 0: - print(f"SCP failed: {result.stderr}") + try: + cmd = ["yt-dlp", url, + "-x", "--audio-format", "mp3", + "-o", self.local_path, + "--no-playlist", "--no-warnings", "--newline"] + print(f"\n▶ yt-dlp {url}") + proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, + text=True, bufsize=1) + for line in proc.stdout: + line = line.rstrip() + if not line: + continue + print(f" [yt-dlp] {line}") + m = self._DL_PROGRESS_RE.search(line) + if m: + self.progress = float(m.group(1)) + self.status = f"Downloading... {self.progress:.0f}%" + elif "ExtractAudio" in line or "Extracting audio" in line: + self.status = "Converting to mp3..." + elif line.startswith("[download] Destination"): + self.status = "Downloading..." + proc.wait(timeout=120) + + if proc.returncode != 0 or not os.path.exists(self.local_path): + print(f"Download failed (exit {proc.returncode})") + self.load_error = "Download failed" return False - self.current_file = remote_path - self.player.playFile(remote_path) + size_mb = os.path.getsize(self.local_path) / (1024 * 1024) + print(f"▶ scp {self.local_path} -> nao@{self.nao_ip}:{self.remote_path} ({size_mb:.1f} MB)") + self.progress = 0.0 + upload_start = time.time() + stop_ticker = threading.Event() + + def ticker(): + while not stop_ticker.is_set(): + elapsed = time.time() - upload_start + self.status = f"Uploading to NAO ({size_mb:.1f} MB)... {elapsed:.0f}s" + stop_ticker.wait(1.0) + + t = threading.Thread(target=ticker, daemon=True) + t.start() + scp_result = subprocess.run( + ["sshpass", "-p", "nao", "scp", self.local_path, f"nao@{self.nao_ip}:{self.remote_path}"], + capture_output=True, text=True, timeout=60 + ) + stop_ticker.set() + t.join(timeout=1.0) + print(f" [scp] exit {scp_result.returncode}" + + (f": {scp_result.stderr.strip()}" if scp_result.stderr.strip() else "")) + if scp_result.returncode != 0: + print(f"SCP failed: {scp_result.stderr.strip()}") + self.load_error = "SCP failed" + return False + + self.current_file = self.remote_path + self.status = "Playing" + self.progress = 100.0 + self.current_task_id = self.player.playFile(self.remote_path) self.is_playing = True - print(f"♪ Playing on NAO: {title}") + print(f"♪ Playing on NAO: {title} (task {self.current_task_id})") return True except Exception as e: print(f"Playback error: {e}") + self.load_error = str(e) return False + finally: + self.is_loading = False def pause(self): - if not self.current_file: return - if self.is_playing: - self.player.pause() - self.is_playing = False - else: - self.player.play(self.current_file) - self.is_playing = True + if self.current_task_id is None or not self.current_file: + print("⏸ Nothing loaded to pause/resume") + return + try: + if self.is_playing: + pos = self.player.getCurrentPosition(self.current_task_id) + self.player.stopAll() + self.paused_position = max(0.0, pos) + self.is_playing = False + print(f"♪ Paused at {self.paused_position:.1f}s") + else: + self.current_task_id = self.player.playFileFromPosition( + self.current_file, self.paused_position, 1.0, 0.0 + ) + self.is_playing = True + print(f"♪ Resumed from {self.paused_position:.1f}s (task {self.current_task_id})") + except Exception as e: + print(f"Pause/resume error: {e}") + self.load_error = f"Pause error: {e}" def stop(self): try: self.player.stopAll() - except: - pass + except Exception as e: + print(f"Stop error: {e}") self.is_playing = False self.current_file = None + self.current_task_id = None + self.paused_position = 0.0 def seek(self, seconds): + if self.current_task_id is None: + return try: - pos = self.player.getCurrentPosition() / 1000.0 - self.player.goTo(pos + seconds) - except: pass + pos = self.player.getCurrentPosition(self.current_task_id) + self.player.goTo(self.current_task_id, max(0.0, pos + seconds)) + except Exception as e: + print(f"Seek error: {e}") # ========================================== @@ -256,6 +338,7 @@ class NaoTeleop: self.chat_message = "" self.music_search_mode = False self.music_search_text = "" + self.music_search_pending = False self.video_scale = video_scale self.display_width = int(320 * video_scale) @@ -343,12 +426,18 @@ class NaoTeleop: 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) + try: + self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3) + except Exception as e: + print(f"Head move error: {e}") def reset_head(self): self.head_yaw = 0.0 self.head_pitch = 0.0 - self.motion.setAngles(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.4) + try: + self.motion.setAngles(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.4) + except Exception as e: + print(f"Head reset error: {e}") def update_title(self): mode = "[TYPING] " if self.typing_mode else "" @@ -359,7 +448,8 @@ class NaoTeleop: if self.audio_device: try: self.audio_device.setOutputVolume(self.volume) - except: pass + except Exception as e: + print(f"Volume set error: {e}") print(f"🔊 NAO volume: {self.volume}%") self.update_title() @@ -381,148 +471,220 @@ class NaoTeleop: carpet_config = [["StepHeight", 0.028], ["MaxStepFrequency", 0.6], ["TorsoWy", 0.05]] - print("\n🎮 Controls: WASD=Walk | IJKL=Head | SPACE=Reset head | 1=Wave | M=Music search | P=Pause | X=Stop | ←→=Seek | T=TTS | ESC=Quit") + print("\n🎮 Controls: WASD/Arrows=Walk | IJKL=Head | SPACE=Reset head | 1=Wave | M=Music search (↑↓ select, Enter confirm) | P=Pause | X=Stop | ,/.=Seek | -/==Volume | T=TTS | ESC=Quit") - while self.running: - self.check_battery() + try: + while self.running: + try: + self.check_battery() - for event in pygame.event.get(): - if event.type == pygame.QUIT: - self.running = False - elif event.type == pygame.KEYDOWN: - if self.music_search_mode: - if event.key == pygame.K_RETURN: - if self.music_search_text.strip(): - self.music_results = self.music.search(self.music_search_text) - if self.music_results: - self.selected_result = 0 - else: - self.music_search_mode = False - else: - self.music_search_mode = False - elif event.key == pygame.K_ESCAPE: - self.music_search_mode = False - self.music_search_text = "" - elif event.key == pygame.K_BACKSPACE: - self.music_search_text = self.music_search_text[:-1] - elif event.key in (pygame.K_1, pygame.K_2, pygame.K_3, pygame.K_4, pygame.K_5): - idx = int(event.unicode) - 1 - if idx < len(self.music_results): - entry = self.music_results[idx] - self.music.play(entry['url'], entry.get('title', 'Unknown')) - self.music_search_mode = False - else: - self.music_search_text += event.unicode - elif self.typing_mode: - if event.key == pygame.K_RETURN: - if self.chat_message.strip(): - threading.Thread(target=self.tts.say, args=(self.chat_message,)).start() - self.chat_message = "" - self.typing_mode = False - elif event.key == pygame.K_ESCAPE: - self.chat_message = "" - self.typing_mode = False - 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: + for event in pygame.event.get(): + if event.type == pygame.QUIT: self.running = False - elif event.key == pygame.K_1: - self.wave_gesture() - elif event.key == pygame.K_SPACE: - self.reset_head() - elif event.key == pygame.K_m: - self.music_search_mode = True - self.music_search_text = "" - self.music_results = [] - elif event.key == pygame.K_p: - self.music.pause() - elif event.key == pygame.K_x: - 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() + elif event.type == pygame.KEYDOWN: + if self.music_search_mode: + if event.key == pygame.K_RETURN: + if self.music_results: + entry = self.music_results[self.selected_result] + threading.Thread( + target=self.music.play, + args=(entry['url'], entry.get('title', 'Unknown')), + daemon=True + ).start() + self.music_search_mode = False + elif self.music_search_text.strip() and not self.music_search_pending: + query = self.music_search_text + self.music_search_pending = True + def do_search(q=query): + results = self.music.search(q) + self.music_results = results + self.selected_result = 0 + self.music_search_pending = False + if not results: + self.music_search_mode = False + threading.Thread(target=do_search, daemon=True).start() + else: + self.music_search_mode = False + elif event.key == pygame.K_ESCAPE: + self.music_search_mode = False + self.music_search_text = "" + self.music_results = [] + elif event.key == pygame.K_UP: + if self.music_results: + self.selected_result = (self.selected_result - 1) % len(self.music_results[:5]) + elif event.key == pygame.K_DOWN: + if self.music_results: + self.selected_result = (self.selected_result + 1) % len(self.music_results[:5]) + elif event.key == pygame.K_BACKSPACE: + self.music_search_text = self.music_search_text[:-1] + self.music_results = [] + elif event.key in (pygame.K_1, pygame.K_2, pygame.K_3, pygame.K_4, pygame.K_5): + idx = int(event.unicode) - 1 + if idx < len(self.music_results): + entry = self.music_results[idx] + threading.Thread( + target=self.music.play, + args=(entry['url'], entry.get('title', 'Unknown')), + daemon=True + ).start() + self.music_search_mode = False + elif event.unicode.isprintable(): + self.music_search_text += event.unicode + self.music_results = [] + elif self.typing_mode: + if event.key == pygame.K_RETURN: + if self.chat_message.strip(): + threading.Thread(target=self.tts.say, args=(self.chat_message,), daemon=True).start() + self.chat_message = "" + self.typing_mode = False + elif event.key == pygame.K_ESCAPE: + self.chat_message = "" + self.typing_mode = False + 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_SPACE: + self.reset_head() + elif event.key == pygame.K_m: + self.music_search_mode = True + self.music_search_text = "" + self.music_results = [] + elif event.key == pygame.K_p: + self.music.pause() + elif event.key == pygame.K_x: + self.music.stop() + elif event.key == pygame.K_COMMA: + self.music.seek(-10) + elif event.key == pygame.K_PERIOD: + self.music.seek(10) + elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS): + self.change_volume(-5) + elif event.key in (pygame.K_EQUALS, pygame.K_KP_PLUS): + self.change_volume(5) + elif event.key == pygame.K_t: + self.typing_mode = True + try: + self.motion.stopMove() + except Exception as e: + print(f"stopMove error: {e}") - if not self.typing_mode and not self.music_search_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 not self.typing_mode and not self.music_search_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() + try: + if abs(x) > 0.1 or abs(theta) > 0.1: + self.motion.moveToward(x, y, theta, carpet_config) + else: + self.motion.stopMove() + except Exception as e: + print(f"Move error: {e}") + self.handle_head_movement() - frame = self.get_image() - if frame is not None: - 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: - self.screen.fill((10, 10, 30)) + frame = self.get_image() + if frame is not None: + 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: + self.screen.fill((10, 10, 30)) - # Music status - if self.music.current_title: - status = f"♪ {self.music.current_title[:45]} {'(Paused)' if not self.music.is_playing else ''}" - text = self.font.render(status, True, (0, 255, 100)) - self.screen.blit(text, (10, 10)) + # Music status + if self.music.is_loading: + text = self.font.render(f"♪ {self.music.status}", True, (255, 200, 0)) + self.screen.blit(text, (10, 10)) + if self.music.progress > 0: + bar_w = 220 + pct = min(self.music.progress, 100) / 100.0 + pygame.draw.rect(self.screen, (70, 70, 70), (10, 32, bar_w, 8)) + pygame.draw.rect(self.screen, (255, 200, 0), (10, 32, int(bar_w * pct), 8)) + elif self.music.load_error: + text = self.font.render(f"♪ Error: {self.music.load_error}", True, (255, 80, 80)) + self.screen.blit(text, (10, 10)) + elif self.music.current_title: + status = f"♪ {self.music.current_title[:45]} {'(Paused)' if not self.music.is_playing else ''}" + text = self.font.render(status, True, (0, 255, 100)) + self.screen.blit(text, (10, 10)) - # Music search interface - if self.music_search_mode: - s = pygame.Surface((self.display_width, self.display_height//2)) - s.set_alpha(220) - s.fill((0, 0, 40)) - self.screen.blit(s, (0, 80)) + # Music search interface + if self.music_search_mode: + box_h = min(self.display_height - 80, 260) + s = pygame.Surface((self.display_width, box_h)) + s.set_alpha(220) + s.fill((0, 0, 40)) + self.screen.blit(s, (0, 80)) - text = self.font.render(f"Search: {self.music_search_text}", True, (255, 255, 255)) - self.screen.blit(text, (20, 100)) + text = self.font.render(f"Search: {self.music_search_text}", True, (255, 255, 255)) + self.screen.blit(text, (20, 100)) - if self.music_results: - for i, entry in enumerate(self.music_results[:5]): - color = (255, 255, 0) if i == self.selected_result else (200, 200, 200) - title = entry.get('title', 'No title')[:60] - line = self.font.render(f"{i+1}. {title}", True, color) - self.screen.blit(line, (20, 140 + i*30)) - else: - text = self.font.render("Searching...", True, (200, 200, 200)) - self.screen.blit(text, (20, 140)) + if self.music_results: + shown = self.music_results[:5] + for i, entry in enumerate(shown): + row_y = 140 + i * 30 + if i == self.selected_result: + pygame.draw.rect(self.screen, (90, 90, 0), (16, row_y - 3, self.display_width - 32, 26)) + color = (255, 255, 0) if i == self.selected_result else (200, 200, 200) + title = entry.get('title', 'No title')[:60] + line = self.font.render(f"{i+1}. {title}", True, color) + self.screen.blit(line, (20, row_y)) + hint_y = 140 + len(shown) * 30 + 6 + hint = self.font.render("\u2191\u2193 Select Enter Confirm 1-5 Quick pick Esc Cancel", True, (150, 150, 150)) + self.screen.blit(hint, (20, hint_y)) + elif self.music_search_pending: + text = self.font.render("Searching...", True, (200, 200, 200)) + self.screen.blit(text, (20, 140)) + else: + text = self.font.render("No results. Type to search again, Enter to search", True, (200, 200, 200)) + self.screen.blit(text, (20, 140)) - # TTS box - 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)) + # TTS box + 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)) - 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 hasattr(self, 'video') and self.video_client: - try: self.video.unsubscribe(self.video_client) - except: pass - self.motion.rest() - pygame.quit() + pygame.display.flip() + self.clock.tick(0) + except Exception as e: + print(f"\u26a0\ufe0f Frame error (continuing): {e}") + finally: + print("🛑 Shutting down...") + try: + self.motion.stopMove() + except Exception as e: + print(f"stopMove error: {e}") + try: + self.music.stop() + except Exception as e: + print(f"music stop error: {e}") + if self.audio_device: + try: self.audio_device.unsubscribe(self.audio_service_name) + except Exception as e: print(f"audio unsubscribe error: {e}") + if getattr(self, "sound_receiver", None): + try: self.sound_receiver.close() + except Exception as e: print(f"sound receiver close error: {e}") + if hasattr(self, 'video') and self.video_client: + try: self.video.unsubscribe(self.video_client) + except Exception as e: print(f"video unsubscribe error: {e}") + try: + self.motion.rest() + except Exception as e: + print(f"rest error: {e}") + pygame.quit() if __name__ == "__main__":