Lucca Pirovano
998 lines
47 KiB
Python
998 lines
47 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
|
|
import os
|
|
import json
|
|
import re
|
|
import socket
|
|
|
|
# Import the new music module
|
|
from naomusic import NaoMusicPlayer
|
|
|
|
# --- Audio for mic ---
|
|
try:
|
|
import pyaudio
|
|
HAS_AUDIO = True
|
|
except ImportError:
|
|
HAS_AUDIO = False
|
|
|
|
# --- SSH for live mic relay (push-to-talk) ---
|
|
try:
|
|
import paramiko
|
|
HAS_SSH = True
|
|
except ImportError:
|
|
HAS_SSH = False
|
|
|
|
# --- HTTP client for the on-robot MJPEG relay (see nao_video_server.py) ---
|
|
try:
|
|
import requests
|
|
HAS_REQUESTS = True
|
|
except ImportError:
|
|
HAS_REQUESTS = False
|
|
|
|
# ==========================================
|
|
# NAO MIC STREAM
|
|
# ==========================================
|
|
class SoundReceiver:
|
|
PREBUFFER_CHUNKS = 3
|
|
SILENCE_CHUNK = b"\x00" * 4096
|
|
|
|
def __init__(self, output_device_index=None, mic_gain=8.0, pyaudio_instance=None):
|
|
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)
|
|
self.muted = False
|
|
self._owns_pyaudio = False
|
|
|
|
if HAS_AUDIO:
|
|
self.p = pyaudio_instance if pyaudio_instance is not None else pyaudio.PyAudio()
|
|
self._owns_pyaudio = pyaudio_instance is None
|
|
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:
|
|
if self.muted:
|
|
self.stream.write(b"\x00" * len(raw_bytes))
|
|
return
|
|
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()
|
|
if self._owns_pyaudio:
|
|
self.p.terminate()
|
|
except: pass
|
|
|
|
# ==========================================
|
|
# LIVE MIC RELAY (push-to-talk over SSH)
|
|
# ==========================================
|
|
class LiveMicRelay:
|
|
"""
|
|
Push-to-talk: streams the local mic straight to NAO's own speaker.
|
|
|
|
connect() (call once at startup, off the hot path) SSHes into the
|
|
robot and launches a self-restarting `nc -l | aplay` loop that stays
|
|
up for the whole session. Each start()/stop() (per push-to-talk
|
|
press) then just opens/closes a plain TCP socket to that already-
|
|
running listener and pipes raw mic audio into it - no SSH handshake
|
|
per press. Closing the socket on stop() ends that iteration's nc,
|
|
which ends aplay with it, and the remote loop immediately relaunches
|
|
nc so it's ready for the next press. close() (call once at shutdown)
|
|
tears down the persistent SSH connection and kills the remote loop.
|
|
"""
|
|
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22,
|
|
port=5555, rate=16000, input_device_index=None, pyaudio_instance=None):
|
|
self.nao_ip = nao_ip
|
|
self.ssh_user = ssh_user
|
|
self.ssh_password = ssh_password
|
|
self.ssh_port = ssh_port
|
|
self.port = port
|
|
self.rate = rate
|
|
self.input_device_index = input_device_index
|
|
|
|
self.active = False
|
|
self.connected = False # remote listener is up, ready for fast local connects
|
|
# "idle" | "connecting" | "live" - read by the HUD to show status
|
|
self.state = "idle"
|
|
self._ssh_client = None
|
|
self._ssh_channel = None
|
|
self._sock = None
|
|
self._stream = None
|
|
self._thread = None
|
|
# Reuse the app's single PyAudio context - a second independent one
|
|
# opened from a background thread crashes PortAudio's PulseAudio
|
|
# backend (pa_atomic_load assertion).
|
|
self._p = pyaudio_instance if pyaudio_instance is not None else (pyaudio.PyAudio() if HAS_AUDIO else None)
|
|
|
|
def connect(self):
|
|
"""One-time setup: SSH in and launch a self-restarting remote
|
|
listener (`nc | aplay` in a loop) that stays up for the whole app
|
|
session. Call this once at startup, in a background thread so it
|
|
doesn't block. After this, start()/stop() per push-to-talk press
|
|
only do a fast local socket connect - no SSH in the hot path.
|
|
"""
|
|
if not HAS_SSH:
|
|
print("🎙️ Live mic needs paramiko: pip install paramiko")
|
|
return
|
|
try:
|
|
self._ssh_client = paramiko.SSHClient()
|
|
self._ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
|
self._ssh_client.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
|
|
password=self.ssh_password, timeout=5)
|
|
self._ssh_channel = self._ssh_client.get_transport().open_session()
|
|
# A pty ties the remote process group to this channel, so
|
|
# closing it in close() reliably kills the loop/nc/aplay
|
|
# instead of leaving an orphaned process on the robot.
|
|
self._ssh_channel.get_pty()
|
|
remote_cmd = (
|
|
f"while true; do nc -l -p {self.port} | "
|
|
f"aplay -q -r {self.rate} -f S16_LE -c 1 -t raw -; done"
|
|
)
|
|
self._ssh_channel.exec_command(remote_cmd)
|
|
time.sleep(0.4) # one-time wait for the first nc to bind
|
|
self.connected = True
|
|
print("🎙️ Talk relay: remote listener ready")
|
|
except Exception as e:
|
|
print(f"🎙️ Talk relay: couldn't set up remote listener ({e})")
|
|
self.connected = False
|
|
|
|
def close(self):
|
|
"""Tear down the persistent SSH connection/listener. Call once at
|
|
app shutdown - not per push-to-talk press."""
|
|
if self._ssh_channel:
|
|
try: self._ssh_channel.close()
|
|
except: pass
|
|
self._ssh_channel = None
|
|
if self._ssh_client:
|
|
try: self._ssh_client.close()
|
|
except: pass
|
|
self._ssh_client = None
|
|
self.connected = False
|
|
|
|
def start(self):
|
|
if self.active:
|
|
return
|
|
if not HAS_AUDIO:
|
|
print("🎙️ Live mic needs pyaudio installed")
|
|
return
|
|
if not self.connected:
|
|
print("🎙️ Talk relay: remote listener not ready yet")
|
|
return
|
|
# If a previous relay thread is still tearing itself down, wait for
|
|
# it to finish before reusing self._stream/_sock for a new one.
|
|
if self._thread and self._thread.is_alive():
|
|
self._thread.join(timeout=2.0)
|
|
self.active = True
|
|
self.state = "connecting"
|
|
self._thread = threading.Thread(target=self._run, daemon=True)
|
|
self._thread.start()
|
|
|
|
def stop(self):
|
|
"""Signal the relay thread to stop and wait for it to exit.
|
|
|
|
Deliberately does NOT touch self._stream/_sock here: this is
|
|
called from the main/render thread on key-up, and closing a
|
|
PortAudio stream from a different thread while the relay thread is
|
|
mid blocking-read() on it is what caused the ALSA segfault on
|
|
release. Only _run() (via _cleanup, in its own thread) closes them.
|
|
"""
|
|
self.active = False
|
|
self.state = "idle"
|
|
if self._thread and self._thread.is_alive() and threading.current_thread() is not self._thread:
|
|
self._thread.join(timeout=2.0)
|
|
|
|
def _cleanup(self):
|
|
"""Per-press teardown: only the local socket/mic stream. The SSH
|
|
connection is persistent now - close() (at app shutdown) is what
|
|
tears that down, not this."""
|
|
if self._sock:
|
|
try: self._sock.close()
|
|
except: pass
|
|
self._sock = None
|
|
if self._stream:
|
|
try:
|
|
self._stream.stop_stream()
|
|
self._stream.close()
|
|
except: pass
|
|
self._stream = None
|
|
|
|
def _run(self):
|
|
try:
|
|
# Remote listener is already up (from connect()) - just make a
|
|
# fast local socket connection, no SSH handshake here.
|
|
self._sock = socket.create_connection((self.nao_ip, self.port), timeout=5)
|
|
|
|
self._stream = self._p.open(format=pyaudio.paInt16, channels=1,
|
|
rate=self.rate, input=True,
|
|
input_device_index=self.input_device_index,
|
|
frames_per_buffer=1024)
|
|
print("🎙️ Talk relay: live")
|
|
if self.active: # could've been stopped while we were still connecting
|
|
self.state = "live"
|
|
|
|
while self.active:
|
|
try:
|
|
chunk = self._stream.read(1024, exception_on_overflow=False)
|
|
self._sock.sendall(chunk)
|
|
except Exception:
|
|
break
|
|
except Exception as e:
|
|
print(f"🎙️ Talk relay error: {e}")
|
|
finally:
|
|
self.active = False
|
|
self.state = "idle"
|
|
self._cleanup()
|
|
print("🎙️ Talk relay: stopped")
|
|
|
|
# ==========================================
|
|
# MAIN TELEOP
|
|
# ==========================================
|
|
class NaoTeleop:
|
|
def __init__(self, session, nao_ip, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0,
|
|
nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_port=5555, relay_rate=16000,
|
|
video_fps=25, video_http_url=None):
|
|
self.session = session
|
|
self.nao_ip = nao_ip
|
|
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, nao_ip, ssh_port=nao_ssh_port)
|
|
self.music_results = []
|
|
self.selected_result = 0
|
|
|
|
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
|
|
|
|
# One shared PyAudio context for the whole app - SoundReceiver (mic
|
|
# playback) and LiveMicRelay (push-to-talk capture) both use it,
|
|
# since two separate contexts crash PortAudio's PulseAudio backend.
|
|
self._pyaudio = pyaudio.PyAudio() if HAS_AUDIO else None
|
|
|
|
self.mic_relay = LiveMicRelay(nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password,
|
|
ssh_port=nao_ssh_port,
|
|
port=relay_port, rate=relay_rate, pyaudio_instance=self._pyaudio)
|
|
self.ptt_active = False
|
|
|
|
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.walk_speed = 0.6
|
|
|
|
self.typing_mode = False
|
|
self.chat_message = ""
|
|
self.music_search_mode = False
|
|
self.music_search_text = ""
|
|
self.music_search_pending = False
|
|
|
|
self.video_scale = video_scale
|
|
self.video_fps = video_fps
|
|
self.video_http_url = video_http_url
|
|
self.display_width = int(320 * video_scale)
|
|
self.display_height = int(240 * video_scale)
|
|
|
|
# Video and movement are decoupled onto their own threads (see
|
|
# _init_video_maxfps / run) so a stalled network RPC to the robot
|
|
# never blocks keyboard input or the render loop.
|
|
self._latest_frame = None
|
|
self._latest_frame_ts = 0.0
|
|
self._video_stop_event = threading.Event()
|
|
self._desired_velocity = (0.0, 0.0, 0.0)
|
|
self._movement_stop_event = threading.Event()
|
|
self._movement_thread = None
|
|
|
|
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 _safe(self, label, fn):
|
|
try:
|
|
return fn()
|
|
except Exception as e:
|
|
print(f"{label} error: {e}")
|
|
return None
|
|
|
|
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):
|
|
if self.video_http_url:
|
|
if not HAS_REQUESTS:
|
|
print("❌ --video-http-url given but 'requests' isn't installed (pip install requests)")
|
|
self.video_client = None
|
|
return
|
|
self.video = None
|
|
self.video_client = "http" # truthy sentinel, unsubscribe() is a no-op for this path
|
|
print("✅ Camera via on-robot MJPEG relay: %s" % self.video_http_url)
|
|
threading.Thread(target=self._video_loop_http, daemon=True).start()
|
|
return
|
|
try:
|
|
self.video = self.session.service("ALVideoDevice")
|
|
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, self.video_fps)
|
|
print("✅ Camera ready")
|
|
threading.Thread(target=self._video_loop, daemon=True).start()
|
|
except:
|
|
print("❌ Camera not available")
|
|
self.video_client = None
|
|
|
|
def _video_loop(self):
|
|
"""Runs on its own thread for the app's lifetime. getImageRemote()
|
|
is a blocking RPC over the network - if a call hangs during a
|
|
WiFi hiccup, this thread just sits waiting on it. The render loop
|
|
never touches this call directly; it only ever reads whatever
|
|
self._latest_frame was last successfully set to, so a stalled
|
|
fetch here can no longer freeze keyboard input or movement."""
|
|
while not self._video_stop_event.is_set():
|
|
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)
|
|
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
|
self._latest_frame_ts = time.time()
|
|
except Exception:
|
|
time.sleep(0.05)
|
|
|
|
def _video_loop_http(self):
|
|
"""Same role as _video_loop, but pulls an already-JPEG-encoded
|
|
MJPEG stream from nao_video_server.py running on the robot,
|
|
instead of raw RGB frames over the qi RPC session - this is
|
|
the whole point of doing the re-encoding on-robot rather than
|
|
shipping raw pixels across the phone tunnel."""
|
|
boundary = b"--frame"
|
|
while not self._video_stop_event.is_set():
|
|
try:
|
|
resp = requests.get(self.video_http_url, stream=True, timeout=5)
|
|
buf = b""
|
|
for chunk in resp.iter_content(chunk_size=4096):
|
|
if self._video_stop_event.is_set():
|
|
return
|
|
buf += chunk
|
|
while True:
|
|
start = buf.find(boundary)
|
|
if start == -1:
|
|
break
|
|
hdr_end = buf.find(b"\r\n\r\n", start)
|
|
if hdr_end == -1:
|
|
break
|
|
m = re.search(rb"Content-Length:\s*(\d+)", buf[start:hdr_end])
|
|
if not m:
|
|
buf = buf[hdr_end + 4:]
|
|
continue
|
|
length = int(m.group(1))
|
|
frame_start = hdr_end + 4
|
|
if len(buf) < frame_start + length:
|
|
break
|
|
jpeg = buf[frame_start:frame_start + length]
|
|
buf = buf[frame_start + length:]
|
|
arr = np.frombuffer(jpeg, dtype=np.uint8)
|
|
frame = cv2.imdecode(arr, cv2.IMREAD_COLOR)
|
|
if frame is not None:
|
|
self._latest_frame = cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
|
self._latest_frame_ts = time.time()
|
|
except Exception as e:
|
|
print(f"video relay error: {e}")
|
|
time.sleep(0.5)
|
|
|
|
def get_image(self):
|
|
"""Non-blocking: returns whatever frame the background video
|
|
thread most recently captured (or None before the first one
|
|
arrives). Never makes a network call itself."""
|
|
return self._latest_frame
|
|
|
|
def _init_mic_stream(self):
|
|
if not self.audio_device: return
|
|
try:
|
|
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
|
pyaudio_instance=self._pyaudio)
|
|
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 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)
|
|
# Relaxed stiffness after completion
|
|
self.motion.setStiffnesses("RArm", 0.3)
|
|
except Exception as e:
|
|
print(f"Wave error: {e}")
|
|
|
|
def cena_gesture(self):
|
|
if self.typing_mode: return
|
|
print("👋 YOU CAN'T SEE ME!")
|
|
try:
|
|
self.tts.say("You can't see me")
|
|
|
|
self.motion.setStiffnesses("RArm", 1.0)
|
|
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
|
|
|
|
# Setup for tight, face-level sweep
|
|
tight_setup = [-0.4, -0.2, 0.0, 1.2, 1.5, 1.0]
|
|
self.motion.angleInterpolation(names, tight_setup, [0.5]*6, True)
|
|
|
|
# The horizontal "backhand" sweep
|
|
self.motion.angleInterpolation(["RElbowYaw"], [0.8], 0.4, True)
|
|
self.motion.angleInterpolation(["RElbowYaw"], [-0.8], 0.6, True)
|
|
self.motion.angleInterpolation(["RElbowYaw"], [0.0], 0.4, True)
|
|
|
|
# Return to neutral
|
|
neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
|
|
self.motion.angleInterpolationWithSpeed(names, neutral, 0.3)
|
|
# Relaxed stiffness after completion
|
|
self.motion.setStiffnesses("RArm", 0.3)
|
|
except Exception as e:
|
|
print(f"Cena gesture error: {e}")
|
|
|
|
def six_seven_gesture(self):
|
|
if self.typing_mode: return
|
|
print("🖐️ SIX... SEVEN...")
|
|
try:
|
|
threading.Thread(target=self.tts.say, args=("six seven",), daemon=True).start()
|
|
|
|
self.motion.setStiffnesses(["RArm", "LArm"], 1.0)
|
|
|
|
# Lock both arms up in front of the body, hands held out like a scale.
|
|
# ElbowYaw is rotated ~90° off zero here - that's what points the elbow's
|
|
# bend axis so flexing tips the forearm forward/down instead of curling
|
|
# it in across the chest. These joints don't move again until the return.
|
|
hold_names = [
|
|
"RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RWristYaw", "RHand",
|
|
"LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LWristYaw", "LHand"
|
|
]
|
|
hold_angles = [
|
|
0.2, -0.2, 1.4, 1.57, 0.6,
|
|
0.2, 0.2, -1.4, -1.57, 0.6
|
|
]
|
|
self.motion.angleInterpolationWithSpeed(hold_names, hold_angles, 0.25)
|
|
|
|
# Only the elbows seesaw now - a small tip back and forth, one arm
|
|
# rising while the other falls, like weighing something in each hand
|
|
# ("six... seven..."). Small amplitude so it stays a wobble, not a fold.
|
|
elbow_names = ["RElbowRoll", "LElbowRoll"]
|
|
r_low, r_high = 0.6, 1.0
|
|
l_low, l_high = -0.6, -1.0
|
|
|
|
elbow_angles = [
|
|
[r_low, r_high, r_low, r_high, r_low, r_high],
|
|
[l_high, l_low, l_high, l_low, l_high, l_low],
|
|
]
|
|
times = [[0.35, 0.7, 1.05, 1.4, 1.75, 2.1]] * 2
|
|
self.motion.angleInterpolation(elbow_names, elbow_angles, times, True)
|
|
|
|
# Return everything to neutral
|
|
neutral_names = hold_names + elbow_names
|
|
neutral = [1.5, -0.15, 1.2, 0.0, 0.0,
|
|
1.5, 0.15, -1.2, 0.0, 0.0,
|
|
0.5, -0.5]
|
|
self.motion.angleInterpolationWithSpeed(neutral_names, neutral, 0.3)
|
|
# Relaxed stiffness after completion
|
|
self.motion.setStiffnesses(["RArm", "LArm"], 0.3)
|
|
except Exception as e:
|
|
print(f"Six seven gesture error: {e}")
|
|
|
|
def handle_head_movement(self):
|
|
if self.typing_mode or self.music_search_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._safe("Head move", lambda: 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._safe("Head reset", lambda: self.motion.setAngles(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.4))
|
|
|
|
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 _movement_loop(self):
|
|
"""Runs on its own thread for the app's lifetime, sending whatever
|
|
velocity the render loop most recently recorded in
|
|
self._desired_velocity. moveToward/stopMove are blocking network
|
|
RPCs - if one hangs during a WiFi hiccup, only this thread stalls;
|
|
the render loop keeps reading keys and updating the target
|
|
velocity so the very next send (as soon as the network unblocks)
|
|
reflects your latest input, not a stale one from before the drop.
|
|
"""
|
|
stable_config = [
|
|
["StepHeight", 0.02],
|
|
["MaxStepFrequency", 0.5],
|
|
["TorsoWy", 0.08],
|
|
["MaxStepX", 0.03]
|
|
]
|
|
while not self._movement_stop_event.is_set():
|
|
x, y, theta = self._desired_velocity
|
|
try:
|
|
if abs(x) > 0.05 or abs(y) > 0.05 or abs(theta) > 0.05:
|
|
self.motion.moveToward(x, y, theta, stable_config)
|
|
else:
|
|
self.motion.stopMove()
|
|
except Exception as e:
|
|
print(f"Move error: {e}")
|
|
time.sleep(0.05) # ~20Hz send rate
|
|
|
|
def run(self):
|
|
print("🤖 Enabling stiffness (no wakeUp stand-up)...")
|
|
self.motion.setStiffnesses("Body", 1.0)
|
|
|
|
# Enable walk arms BEFORE forcing stiffness
|
|
print("💪 Enabling walk arms...")
|
|
self.motion.setMoveArmsEnabled(True, True)
|
|
|
|
# Force relaxed stiffness AFTER engine is active
|
|
self.motion.setStiffnesses(["RArm", "LArm"], 0.3)
|
|
|
|
print("🦶 Initializing walk engine...")
|
|
self._safe("moveInit", self.motion.moveInit)
|
|
print("🦶 Walk engine ready")
|
|
|
|
if getattr(self, "mic_relay", None):
|
|
threading.Thread(target=self.mic_relay.connect, daemon=True).start()
|
|
|
|
self._movement_thread = threading.Thread(target=self._movement_loop, daemon=True)
|
|
self._movement_thread.start()
|
|
|
|
print("\n🎮 Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | 2=Cena | 3=6-7 | M=Music search | X=Stop | -/==Volume | T=TTS | V=Push-to-talk (hold) | 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_2:
|
|
self.cena_gesture()
|
|
elif event.key == pygame.K_3:
|
|
self.six_seven_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_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()
|
|
|
|
if keys[pygame.K_v] and not self.ptt_active:
|
|
self.ptt_active = True
|
|
self.mic_relay.start()
|
|
if getattr(self, "sound_receiver", None):
|
|
self.sound_receiver.muted = True
|
|
elif not keys[pygame.K_v] and self.ptt_active:
|
|
self.ptt_active = False
|
|
self.mic_relay.stop()
|
|
if getattr(self, "sound_receiver", None):
|
|
self.sound_receiver.muted = False
|
|
|
|
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
|
|
|
|
# Just record intent here - the movement thread
|
|
# (see _movement_loop) is what actually calls
|
|
# moveToward/stopMove, so a stalled network RPC
|
|
# can't stop us reading the next key state.
|
|
self._desired_velocity = (x, y, theta)
|
|
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))
|
|
if time.time() - self._latest_frame_ts > 1.0:
|
|
warn = self.font.render("⚠ VIDEO STALE - reconnecting...", True, (255, 60, 60))
|
|
self.screen.blit(warn, (10, self.display_height - 30))
|
|
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:
|
|
text = self.font.render(f"♪ {self.music.current_title[:45]}", True, (0, 255, 100))
|
|
self.screen.blit(text, (10, 10))
|
|
|
|
# Voice chat HUD (top-right) - only shown while V is held / connecting
|
|
relay_state = getattr(self.mic_relay, "state", "idle")
|
|
if relay_state == "connecting":
|
|
label, color = "VOICE: CONNECTING...", (255, 200, 0)
|
|
elif relay_state == "live":
|
|
label, color = "VOICE: LIVE - speak now", (0, 255, 100)
|
|
else:
|
|
label, color = None, None
|
|
|
|
if label:
|
|
text = self.font.render(label, True, color)
|
|
pad = 8
|
|
box_w, box_h = text.get_width() + pad * 2 + 18, text.get_height() + pad
|
|
box_x = self.display_width - box_w - 10
|
|
box_y = 10
|
|
s = pygame.Surface((box_w, box_h))
|
|
s.set_alpha(180)
|
|
s.fill((0, 0, 0))
|
|
self.screen.blit(s, (box_x, box_y))
|
|
# pulsing dot for "live" so it's easy to catch out of the corner of your eye
|
|
if relay_state == "live":
|
|
pulse = 0.5 + 0.5 * abs((pygame.time.get_ticks() % 1000) / 500.0 - 1.0)
|
|
dot_color = tuple(int(c * pulse) for c in color)
|
|
else:
|
|
dot_color = color
|
|
pygame.draw.circle(self.screen, dot_color, (box_x + pad + 5, box_y + box_h // 2), 5)
|
|
self.screen.blit(text, (box_x + pad + 18, box_y + pad // 2))
|
|
|
|
# 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._movement_stop_event.set()
|
|
if self._movement_thread:
|
|
self._movement_thread.join(timeout=1.0)
|
|
self._video_stop_event.set()
|
|
self._safe("stopMove", self.motion.stopMove)
|
|
self._safe("music stop", self.music.stop)
|
|
if getattr(self, "mic_relay", None):
|
|
self._safe("mic relay stop", self.mic_relay.stop)
|
|
self._safe("mic relay close", self.mic_relay.close)
|
|
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 is not None and self.video_client:
|
|
self._safe("video unsubscribe", lambda: self.video.unsubscribe(self.video_client))
|
|
if getattr(self, "_pyaudio", None):
|
|
self._safe("pyaudio terminate", self._pyaudio.terminate)
|
|
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)
|
|
parser.add_argument("--video-fps", type=int, default=25,
|
|
help="Camera fps requested from NAO. Video is uncompressed RGB at "
|
|
"160x120, so this is roughly 460KB/s per fps - lower it "
|
|
"(e.g. 10-12) when tunneling over a constrained link. "
|
|
"Ignored when --video-http-url is set (the server controls fps).")
|
|
parser.add_argument("--video-http-url", type=str, default=None,
|
|
help="URL of the on-robot MJPEG relay (nao_video_server.py), e.g. "
|
|
"http://127.0.0.1:8000/video when tunneled through the phone. "
|
|
"When set, video comes from this compressed HTTP stream instead "
|
|
"of raw frames over the qi session - much lighter over a relay.")
|
|
parser.add_argument("--nao-ssh-user", type=str, default="nao",
|
|
help="SSH login for the live mic relay (push-to-talk)")
|
|
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
|
help="SSH password for the live mic relay (push-to-talk)")
|
|
parser.add_argument("--nao-ssh-port", type=int, default=22,
|
|
help="SSH port for the robot (music upload + push-to-talk). "
|
|
"Set this to your local forwarded port, e.g. 2222, when tunneling.")
|
|
parser.add_argument("--relay-port", type=int, default=5555,
|
|
help="TCP port used to stream mic audio to the robot's nc listener")
|
|
parser.add_argument("--relay-rate", type=int, default=16000,
|
|
help="Sample rate for the live mic relay - try 48000 if audio sounds sped up/slow")
|
|
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,
|
|
video_fps=args.video_fps,
|
|
video_http_url=args.video_http_url,
|
|
nao_ssh_user=args.nao_ssh_user,
|
|
nao_ssh_password=args.nao_ssh_password,
|
|
nao_ssh_port=args.nao_ssh_port,
|
|
relay_port=args.relay_port,
|
|
relay_rate=args.relay_rate).run()
|