Lucca Pirovano
497 lines
19 KiB
Python
497 lines
19 KiB
Python
#!/usr/bin/env python
|
|
# -*- encoding: UTF-8 -*-
|
|
|
|
import qi
|
|
import argparse
|
|
import sys
|
|
import time
|
|
import threading
|
|
import queue
|
|
import cv2
|
|
import numpy as np
|
|
import pygame
|
|
import subprocess
|
|
|
|
# --- Audio for mic ---
|
|
try:
|
|
import pyaudio
|
|
HAS_AUDIO = True
|
|
except ImportError:
|
|
HAS_AUDIO = False
|
|
print("⚠️ PyAudio not installed.")
|
|
|
|
# ==========================================
|
|
# NAO MIC STREAM TO LAPTOP
|
|
# ==========================================
|
|
class SoundReceiver:
|
|
PREBUFFER_CHUNKS = 3
|
|
SILENCE_CHUNK = b"\x00" * 4096
|
|
|
|
def __init__(self, output_device_index=None, mic_gain=8.0):
|
|
self.running = True
|
|
self.underrun_count = 0
|
|
self.chunks_received = 0
|
|
self._write_errors = 0
|
|
self._peak_since_print = 0
|
|
self._last_status_print = 0
|
|
self.mic_gain = float(mic_gain)
|
|
|
|
if HAS_AUDIO:
|
|
self.p = pyaudio.PyAudio()
|
|
print("🔈 Available output devices:")
|
|
for i in range(self.p.get_device_count()):
|
|
info = self.p.get_device_info_by_index(i)
|
|
if info.get("maxOutputChannels", 0) > 0:
|
|
print(f" [{i}] {info['name']}")
|
|
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: [{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,
|
|
frames_per_buffer=2048)
|
|
self._play_test_tone()
|
|
self.queue = queue.Queue(maxsize=40)
|
|
self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True)
|
|
self.playback_thread.start()
|
|
|
|
def _play_test_tone(self):
|
|
try:
|
|
duration, freq, rate = 0.3, 440.0, 16000
|
|
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())
|
|
except: pass
|
|
|
|
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
|
if not HAS_AUDIO: return
|
|
self.chunks_received += 1
|
|
try:
|
|
self.queue.put_nowait(bytes(buffer))
|
|
except queue.Full:
|
|
try:
|
|
self.queue.get_nowait()
|
|
self.queue.put_nowait(bytes(buffer))
|
|
except: pass
|
|
|
|
def _playback_loop(self):
|
|
primed = []
|
|
while self.running and len(primed) < self.PREBUFFER_CHUNKS:
|
|
try:
|
|
primed.append(self.queue.get(timeout=1.0))
|
|
except queue.Empty:
|
|
break
|
|
for chunk in primed:
|
|
self._write_chunk(chunk)
|
|
|
|
while self.running:
|
|
try:
|
|
chunk = self.queue.get(timeout=0.2)
|
|
self._write_chunk(chunk)
|
|
except queue.Empty:
|
|
self.underrun_count += 1
|
|
try:
|
|
self.stream.write(self.SILENCE_CHUNK)
|
|
except: pass
|
|
|
|
now = time.time()
|
|
if now - self._last_status_print > 10:
|
|
self._last_status_print = now
|
|
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):
|
|
try:
|
|
samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32)
|
|
if samples.size:
|
|
amplified = np.clip(samples * self.mic_gain, -32768, 32767)
|
|
peak = int(np.abs(amplified).max())
|
|
if peak > self._peak_since_print:
|
|
self._peak_since_print = peak
|
|
self.stream.write(amplified.astype(np.int16).tobytes())
|
|
else:
|
|
self.stream.write(raw_bytes)
|
|
except: pass
|
|
|
|
def close(self):
|
|
self.running = False
|
|
if HAS_AUDIO:
|
|
try: self.playback_thread.join(timeout=1.0)
|
|
except: pass
|
|
try:
|
|
self.stream.stop_stream()
|
|
self.stream.close()
|
|
self.p.terminate()
|
|
except: pass
|
|
|
|
|
|
# ==========================================
|
|
# 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):
|
|
self.session = session
|
|
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.music = NaoMusicPlayer(session) # Music on NAO
|
|
|
|
self.audio_device = None
|
|
self.audio_service_name = "SoundReceiver"
|
|
self.audio_device_index = audio_device_index
|
|
self.mic_channel = mic_channel
|
|
self.mic_gain = mic_gain
|
|
|
|
self.running = True
|
|
self.head_yaw = 0.0
|
|
self.head_pitch = 0.0
|
|
self.battery_level = 100
|
|
self.last_batt_check = 0
|
|
self.volume = 50
|
|
|
|
self.typing_mode = False
|
|
self.chat_message = ""
|
|
|
|
self.video_scale = video_scale
|
|
self.display_width = int(320 * video_scale)
|
|
self.display_height = int(240 * video_scale)
|
|
|
|
self._init_audio_device()
|
|
self._init_video_maxfps()
|
|
if HAS_AUDIO:
|
|
self._init_mic_stream()
|
|
|
|
pygame.init()
|
|
self.screen = pygame.display.set_mode((self.display_width, self.display_height))
|
|
pygame.display.set_caption("NAO Teleop + Music on Robot")
|
|
self.font = pygame.font.SysFont(None, 24)
|
|
self.clock = pygame.time.Clock()
|
|
self.update_title()
|
|
|
|
def _init_audio_device(self):
|
|
try:
|
|
self.audio_device = self.session.service("ALAudioDevice")
|
|
self.volume = self.audio_device.getOutputVolume()
|
|
print(f"🔊 NAO speaker volume: {self.volume}%")
|
|
except Exception as e:
|
|
print(f"⚠️ ALAudioDevice error: {e}")
|
|
self.audio_device = None
|
|
|
|
def _init_video_maxfps(self):
|
|
try:
|
|
self.video = self.session.service("ALVideoDevice")
|
|
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25)
|
|
print("✅ Camera ready")
|
|
except:
|
|
print("❌ Camera not available")
|
|
self.video_client = None
|
|
|
|
def _init_mic_stream(self):
|
|
if not self.audio_device: return
|
|
try:
|
|
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"✅ Mic stream ready (gain={self.mic_gain}x)")
|
|
except Exception as e:
|
|
print(f"❌ Mic init failed: {e}")
|
|
|
|
def get_image(self):
|
|
if not hasattr(self, 'video') or not self.video_client:
|
|
return None
|
|
try:
|
|
image = self.video.getImageRemote(self.video_client)
|
|
if image and len(image) >= 7:
|
|
w, h = image[0], image[1]
|
|
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
|
|
frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
|
return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
|
except:
|
|
pass
|
|
return None
|
|
|
|
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]*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 = [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:
|
|
print(f"Wave error: {e}")
|
|
|
|
def handle_head_movement(self):
|
|
if self.typing_mode: return
|
|
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 changed:
|
|
self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)
|
|
|
|
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)
|
|
print("Head reset to neutral")
|
|
|
|
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}%")
|
|
|
|
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: pass
|
|
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)
|
|
|
|
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")
|
|
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()
|
|
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.QUIT:
|
|
self.running = False
|
|
elif event.type == pygame.KEYDOWN:
|
|
if 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
|
|
elif event.key == pygame.K_1:
|
|
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()
|
|
|
|
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_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:
|
|
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))
|
|
|
|
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()
|
|
|
|
|
|
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,
|
|
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()
|