luccapirovano/naowalk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Lucca Pirovano
2026-07-15 18:55:56 -04:00
parent 043c4c3f09
commit c69f8ca7d0
1 changed files with 343 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
naowalk.py
+342 -180
View File
@@ -10,9 +10,10 @@ import queue
import cv2 import cv2
import numpy as np import numpy as np
import pygame import pygame
import yt_dlp
import subprocess import subprocess
import os import os
import json
import re
# --- Audio for mic --- # --- Audio for mic ---
try: try:
@@ -137,87 +138,168 @@ class NaoMusicPlayer:
self.player = session.service("ALAudioPlayer") self.player = session.service("ALAudioPlayer")
self.nao_ip = nao_ip self.nao_ip = nao_ip
self.music_dir = "/home/nao/music" self.music_dir = "/home/nao/music"
self.local_path = "/tmp/song.mp3"
self.remote_path = f"{self.music_dir}/song.mp3"
try: 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) stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
except: pass except: pass
self.current_file = None self.current_file = None
self.current_task_id = None
self.is_playing = False self.is_playing = False
self.paused_position = 0.0
self.current_title = "" self.current_title = ""
self.is_loading = False
self.load_error = None
self.status = ""
self.progress = 0.0
def search(self, query): def search(self, query):
try: try:
ydl_opts = { cmd = ["yt-dlp", f"ytsearch5:{query}", "--flat-playlist", "--dump-json",
'format': 'bestaudio/best', "--no-warnings", "--quiet"]
'quiet': True, result = subprocess.run(cmd, capture_output=True, text=True, timeout=30)
'noplaylist': True, entries = []
'extract_flat': True, for line in result.stdout.strip().splitlines():
} if not line.strip():
with yt_dlp.YoutubeDL(ydl_opts) as ydl: continue
info = ydl.extract_info(f"ytsearch5:{query}", download=False) try:
return info.get('entries', []) data = json.loads(line)
except: 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 [] return []
_DL_PROGRESS_RE = re.compile(r'\[download\]\s+([\d.]+)%')
def play(self, url, title): def play(self, url, title):
self.stop() self.stop()
self.current_title = title self.current_title = title
self.is_loading = True
self.load_error = None
self.status = "Starting download..."
self.progress = 0.0
try: try:
ydl_opts = { if os.path.exists(self.local_path):
'format': 'bestaudio/best', os.remove(self.local_path)
'outtmpl': '/tmp/%(id)s.%(ext)s', except: pass
'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)
remote_path = f"{self.music_dir}/{int(time.time())}.mp3" try:
result = subprocess.run(["sshpass", "-p", "nao", "scp", downloaded, f"nao@{self.nao_ip}:{remote_path}"], cmd = ["yt-dlp", url,
capture_output=True, text=True) "-x", "--audio-format", "mp3",
if result.returncode != 0: "-o", self.local_path,
print(f"SCP failed: {result.stderr}") "--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 return False
self.current_file = remote_path size_mb = os.path.getsize(self.local_path) / (1024 * 1024)
self.player.playFile(remote_path) 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 self.is_playing = True
print(f"♪ Playing on NAO: {title}") print(f"♪ Playing on NAO: {title} (task {self.current_task_id})")
return True return True
except Exception as e: except Exception as e:
print(f"Playback error: {e}") print(f"Playback error: {e}")
self.load_error = str(e)
return False return False
finally:
self.is_loading = False
def pause(self): def pause(self):
if not self.current_file: return if self.current_task_id is None or not self.current_file:
if self.is_playing: print("⏸ Nothing loaded to pause/resume")
self.player.pause() return
self.is_playing = False try:
else: if self.is_playing:
self.player.play(self.current_file) pos = self.player.getCurrentPosition(self.current_task_id)
self.is_playing = True 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): def stop(self):
try: try:
self.player.stopAll() self.player.stopAll()
except: except Exception as e:
pass print(f"Stop error: {e}")
self.is_playing = False self.is_playing = False
self.current_file = None self.current_file = None
self.current_task_id = None
self.paused_position = 0.0
def seek(self, seconds): def seek(self, seconds):
if self.current_task_id is None:
return
try: try:
pos = self.player.getCurrentPosition() / 1000.0 pos = self.player.getCurrentPosition(self.current_task_id)
self.player.goTo(pos + seconds) self.player.goTo(self.current_task_id, max(0.0, pos + seconds))
except: pass except Exception as e:
print(f"Seek error: {e}")
# ========================================== # ==========================================
@@ -256,6 +338,7 @@ class NaoTeleop:
self.chat_message = "" self.chat_message = ""
self.music_search_mode = False self.music_search_mode = False
self.music_search_text = "" self.music_search_text = ""
self.music_search_pending = False
self.video_scale = video_scale self.video_scale = video_scale
self.display_width = int(320 * 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_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_l]: self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True
if changed: 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): def reset_head(self):
self.head_yaw = 0.0 self.head_yaw = 0.0
self.head_pitch = 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): def update_title(self):
mode = "[TYPING] " if self.typing_mode else "" mode = "[TYPING] " if self.typing_mode else ""
@@ -359,7 +448,8 @@ class NaoTeleop:
if self.audio_device: if self.audio_device:
try: try:
self.audio_device.setOutputVolume(self.volume) self.audio_device.setOutputVolume(self.volume)
except: pass except Exception as e:
print(f"Volume set error: {e}")
print(f"🔊 NAO volume: {self.volume}%") print(f"🔊 NAO volume: {self.volume}%")
self.update_title() self.update_title()
@@ -381,148 +471,220 @@ class NaoTeleop:
carpet_config = [["StepHeight", 0.028], ["MaxStepFrequency", 0.6], ["TorsoWy", 0.05]] 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: try:
self.check_battery() while self.running:
try:
self.check_battery()
for event in pygame.event.get(): for event in pygame.event.get():
if event.type == pygame.QUIT: 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:
self.running = False self.running = False
elif event.key == pygame.K_1: elif event.type == pygame.KEYDOWN:
self.wave_gesture() if self.music_search_mode:
elif event.key == pygame.K_SPACE: if event.key == pygame.K_RETURN:
self.reset_head() if self.music_results:
elif event.key == pygame.K_m: entry = self.music_results[self.selected_result]
self.music_search_mode = True threading.Thread(
self.music_search_text = "" target=self.music.play,
self.music_results = [] args=(entry['url'], entry.get('title', 'Unknown')),
elif event.key == pygame.K_p: daemon=True
self.music.pause() ).start()
elif event.key == pygame.K_x: self.music_search_mode = False
self.music.stop() elif self.music_search_text.strip() and not self.music_search_pending:
elif event.key == pygame.K_LEFT: query = self.music_search_text
self.music.seek(-10) self.music_search_pending = True
elif event.key == pygame.K_RIGHT: def do_search(q=query):
self.music.seek(10) results = self.music.search(q)
elif event.key == pygame.K_t: self.music_results = results
self.typing_mode = True self.selected_result = 0
self.motion.stopMove() 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: if not self.typing_mode and not self.music_search_mode:
keys = pygame.key.get_pressed() keys = pygame.key.get_pressed()
x = y = theta = 0.0 x = y = theta = 0.0
if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.6 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_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_a] or keys[pygame.K_LEFT]: theta = 0.5
if keys[pygame.K_d] or keys[pygame.K_RIGHT]: 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: try:
self.motion.moveToward(x, y, theta, carpet_config) if abs(x) > 0.1 or abs(theta) > 0.1:
else: self.motion.moveToward(x, y, theta, carpet_config)
self.motion.stopMove() else:
self.handle_head_movement() self.motion.stopMove()
except Exception as e:
print(f"Move error: {e}")
self.handle_head_movement()
frame = self.get_image() frame = self.get_image()
if frame is not None: if frame is not None:
up = cv2.resize(frame, (self.display_width, self.display_height), interpolation=cv2.INTER_CUBIC) up = cv2.resize(frame, (self.display_width, self.display_height), interpolation=cv2.INTER_CUBIC)
rgb = cv2.cvtColor(up, cv2.COLOR_BGR2RGB) rgb = cv2.cvtColor(up, cv2.COLOR_BGR2RGB)
surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1)) surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1))
self.screen.blit(surf, (0, 0)) self.screen.blit(surf, (0, 0))
else: else:
self.screen.fill((10, 10, 30)) self.screen.fill((10, 10, 30))
# Music status # Music status
if self.music.current_title: if self.music.is_loading:
status = f"♪ {self.music.current_title[:45]} {'(Paused)' if not self.music.is_playing else ''}" text = self.font.render(f"♪ {self.music.status}", True, (255, 200, 0))
text = self.font.render(status, True, (0, 255, 100)) self.screen.blit(text, (10, 10))
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 # Music search interface
if self.music_search_mode: if self.music_search_mode:
s = pygame.Surface((self.display_width, self.display_height//2)) box_h = min(self.display_height - 80, 260)
s.set_alpha(220) s = pygame.Surface((self.display_width, box_h))
s.fill((0, 0, 40)) s.set_alpha(220)
self.screen.blit(s, (0, 80)) 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)) text = self.font.render(f"Search: {self.music_search_text}", True, (255, 255, 255))
self.screen.blit(text, (20, 100)) self.screen.blit(text, (20, 100))
if self.music_results: if self.music_results:
for i, entry in enumerate(self.music_results[:5]): shown = self.music_results[:5]
color = (255, 255, 0) if i == self.selected_result else (200, 200, 200) for i, entry in enumerate(shown):
title = entry.get('title', 'No title')[:60] row_y = 140 + i * 30
line = self.font.render(f"{i+1}. {title}", True, color) if i == self.selected_result:
self.screen.blit(line, (20, 140 + i*30)) pygame.draw.rect(self.screen, (90, 90, 0), (16, row_y - 3, self.display_width - 32, 26))
else: color = (255, 255, 0) if i == self.selected_result else (200, 200, 200)
text = self.font.render("Searching...", True, (200, 200, 200)) title = entry.get('title', 'No title')[:60]
self.screen.blit(text, (20, 140)) 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 # TTS box
if self.typing_mode: if self.typing_mode:
s = pygame.Surface((self.display_width, 40)) s = pygame.Surface((self.display_width, 40))
s.set_alpha(180) s.set_alpha(180)
s.fill((0,0,0)) s.fill((0,0,0))
self.screen.blit(s, (0, self.display_height-40)) self.screen.blit(s, (0, self.display_height-40))
text = self.font.render(f"Say: {self.chat_message}", True, (255,255,255)) text = self.font.render(f"Say: {self.chat_message}", True, (255,255,255))
self.screen.blit(text, (10, self.display_height-30)) self.screen.blit(text, (10, self.display_height-30))
pygame.display.flip() pygame.display.flip()
self.clock.tick(0) self.clock.tick(0)
except Exception as e:
print("🛑 Shutting down...") print(f"\u26a0\ufe0f Frame error (continuing): {e}")
self.motion.stopMove() finally:
self.music.stop() print("🛑 Shutting down...")
if self.audio_device: try:
try: self.audio_device.unsubscribe(self.audio_service_name) self.motion.stopMove()
except: pass except Exception as e:
if getattr(self, "sound_receiver", None): print(f"stopMove error: {e}")
self.sound_receiver.close() try:
if hasattr(self, 'video') and self.video_client: self.music.stop()
try: self.video.unsubscribe(self.video_client) except Exception as e:
except: pass print(f"music stop error: {e}")
self.motion.rest() if self.audio_device:
pygame.quit() 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__": if __name__ == "__main__":