added naomusic.py and moved all music stuff to it
This commit is contained in:
Lucca Pirovano
+147
@@ -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 = ""
|
||||||
+4
-441
@@ -10,10 +10,9 @@ import queue
|
|||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import pygame
|
import pygame
|
||||||
import subprocess
|
|
||||||
import os
|
# Import the new music module
|
||||||
import json
|
from naomusic import NaoMusicPlayer
|
||||||
import re
|
|
||||||
|
|
||||||
# --- Audio for mic ---
|
# --- Audio for mic ---
|
||||||
try:
|
try:
|
||||||
@@ -128,172 +127,6 @@ class SoundReceiver:
|
|||||||
self.p.terminate()
|
self.p.terminate()
|
||||||
except: pass
|
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
|
# MAIN TELEOP
|
||||||
# ==========================================
|
# ==========================================
|
||||||
@@ -437,274 +270,4 @@ class NaoTeleop:
|
|||||||
|
|
||||||
def update_title(self):
|
def update_title(self):
|
||||||
mode = "[TYPING] " if self.typing_mode else ""
|
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}")
|
pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol
|
||||||
|
|
||||||
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()
|
|
||||||
|
|||||||
Reference in New Issue
Block a user