diff --git a/naomusic.py b/naomusic.py new file mode 100644 index 0000000..2aaafb9 --- /dev/null +++ b/naomusic.py @@ -0,0 +1,147 @@ +# naomusic.py +# -*- encoding: UTF-8 -*- + +import subprocess +import os +import json +import re +import time +import threading + +class NaoMusicPlayer: + def __init__(self, session, nao_ip): + self.session = session + 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}"], + stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + except: pass + self.current_file = None + self.current_task_id = None + self.is_playing = False + self.current_title = "" + self.is_loading = False + self.load_error = None + self.status = "" + self.progress = 0.0 + + def search(self, query): + try: + 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: + if os.path.exists(self.local_path): + os.remove(self.local_path) + except: pass + + 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 + + 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} (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 stop(self): + try: + self.player.stopAll() + except Exception as e: + print(f"Stop error: {e}") + self.is_playing = False + self.current_file = None + self.current_task_id = None + self.current_title = "" diff --git a/naowalk.py b/naowalk.py index 194d0b6..6b8b0f0 100644 --- a/naowalk.py +++ b/naowalk.py @@ -10,10 +10,9 @@ import queue import cv2 import numpy as np import pygame -import subprocess -import os -import json -import re + +# Import the new music module +from naomusic import NaoMusicPlayer # --- Audio for mic --- try: @@ -128,172 +127,6 @@ class SoundReceiver: self.p.terminate() except: pass - -# ========================================== -# MUSIC PLAYER ON NAO (scp) -# ========================================== -class NaoMusicPlayer: - def __init__(self, session, nao_ip): - self.session = session - 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}"], - stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) - except: pass - self.current_file = None - self.current_task_id = None - self.is_playing = False - self.current_title = "" - self.is_loading = False - self.load_error = None - self.status = "" - self.progress = 0.0 - - def search(self, query): - try: - 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: - if os.path.exists(self.local_path): - os.remove(self.local_path) - except: pass - - 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 - - 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} (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): - """Toggle playback. No position tracking: resuming restarts the track from the top.""" - if self.current_task_id is None or not self.current_file: - print("⏸ Nothing loaded to pause/resume") - return - if self.is_playing: - try: - self.player.stopAll() - except Exception as e: - print(f"stopAll error while pausing: {e}") - self.load_error = f"Pause error: {e}" - return - self.is_playing = False - print("♪ Paused") - else: - try: - self.current_task_id = self.player.playFile(self.current_file) - self.is_playing = True - print(f"♪ Resumed from start (task {self.current_task_id})") - except Exception as e: - print(f"Resume error: {e}") - self.load_error = f"Resume error: {e}" - - def stop(self): - try: - self.player.stopAll() - except Exception as e: - print(f"Stop error: {e}") - self.is_playing = False - self.current_file = None - self.current_task_id = None - self.current_title = "" - - # ========================================== # MAIN TELEOP # ========================================== @@ -437,274 +270,4 @@ 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}% | Vol: {self.volume}% | Speed: {self.walk_speed:.2f}") - - def change_volume(self, delta): - self.volume = int(min(100, max(0, self.volume + delta))) - if self.audio_device: - self._safe("Volume set", lambda: self.audio_device.setOutputVolume(self.volume)) - print(f"🔊 NAO 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) - - print("🦶 Initializing walk engine...") - self._safe("moveInit", self.motion.moveInit) - print("🦶 Walk engine ready") - - print("\n🎮 Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | M=Music search (↑↓ select, Enter confirm) | P=Pause | X=Stop | -/==Volume | T=TTS | ESC=Quit") - - 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_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 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 - self._safe("stopMove", self.motion.stopMove) - - if not self.typing_mode and not self.music_search_mode: - keys = pygame.key.get_pressed() - - # Speed Adjustment with Title Bar Update - speed_changed = False - if keys[pygame.K_LEFTBRACKET]: - self.walk_speed = max(0.1, self.walk_speed - 0.01) - speed_changed = True - if keys[pygame.K_RIGHTBRACKET]: - self.walk_speed = min(1.0, self.walk_speed + 0.01) - speed_changed = True - if speed_changed: - self.update_title() - - x = y = theta = 0.0 - if keys[pygame.K_w] or keys[pygame.K_UP]: x = self.walk_speed - if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -self.walk_speed - if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = self.walk_speed * 0.8 - if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -self.walk_speed * 0.8 - - try: - if abs(x) > 0.05 or abs(theta) > 0.05: - # Fixed stable configuration mapping - stable_config = [ - ["StepHeight", 0.02], - ["MaxStepFrequency", 0.5], - ["TorsoWy", 0.08], - ["MaxStepX", 0.03] # Replaced MaxStepLength - ] - # Removed the forced stiffness block here - self.motion.moveToward(x, y, theta, stable_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)) - - # 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: - 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)) - - 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)) - - pygame.display.flip() - self.clock.tick(0) - except Exception as e: - print(f"\u26a0\ufe0f Frame error (continuing): {e}") - finally: - print("🛑 Shutting down...") - self._safe("stopMove", self.motion.stopMove) - self._safe("music stop", self.music.stop) - if self.audio_device: - self._safe("audio unsubscribe", lambda: self.audio_device.unsubscribe(self.audio_service_name)) - if getattr(self, "sound_receiver", None): - self._safe("sound receiver close", self.sound_receiver.close) - if hasattr(self, 'video') and self.video_client: - self._safe("video unsubscribe", lambda: self.video.unsubscribe(self.video_client)) - self._safe("rest", 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) - parser.add_argument("--video-scale", type=float, default=2.0) - 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, nao_ip=args.ip, - audio_device_index=args.audio_device_index, - mic_channel=MIC_CHANNELS[args.mic_channel], - mic_gain=args.mic_gain, - video_scale=args.video_scale).run() + pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol