Lucca Pirovano
1321 lines
62 KiB
Python
1321 lines
62 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 struct
|
|
import socket
|
|
import importlib.util
|
|
import shutil
|
|
import tempfile
|
|
import inspect
|
|
|
|
# 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
|
|
|
|
# ==========================================
|
|
# MEDIA RELAY WIRE PROTOCOL (see nao_video_server.py)
|
|
# One video connection (server->client only) and one audio connection
|
|
# (bidirectional - server->client AUDIO is the robot's own mic,
|
|
# client->server TALK is push-to-talk audio to play on the robot's
|
|
# speaker), both length-framed: [1 byte type][4 byte length][payload].
|
|
# Replaces both the old HTTP MJPEG pull AND the old SSH `nc | aplay`
|
|
# push-to-talk relay - everything audio/video now goes through this
|
|
# one pair of connections.
|
|
# ==========================================
|
|
TYPE_HELLO = 0x00
|
|
TYPE_VIDEO = 0x01
|
|
TYPE_AUDIO = 0x02
|
|
TYPE_TALK = 0x03
|
|
|
|
|
|
def decode_ulaw(q):
|
|
"""uint8 mu-law samples -> int16 PCM. Must match encode_ulaw() in
|
|
nao_video_server.py exactly, or this just produces noise."""
|
|
mu = 255.0
|
|
y = (q.astype(np.float64) / 255.0) * 2.0 - 1.0
|
|
x = np.sign(y) * (1.0 / mu) * (np.power(1.0 + mu, np.abs(y)) - 1.0)
|
|
return np.clip(x * 32768.0, -32768, 32767).astype(np.int16)
|
|
|
|
|
|
def encode_ulaw(pcm16):
|
|
"""int16 numpy array -> uint8 numpy array, mu-law companded. Mirror
|
|
of nao_video_server.py's encode_ulaw() - used here now to compress
|
|
push-to-talk audio before it crosses the (possibly tunneled) link,
|
|
the same way the robot's own mic audio already was. Must match
|
|
nao_video_server.py's decode_ulaw() exactly."""
|
|
mu = 255.0
|
|
x = np.clip(pcm16.astype(np.float64) / 32768.0, -1.0, 1.0)
|
|
y = np.sign(x) * np.log1p(mu * np.abs(x)) / np.log1p(mu)
|
|
return (((y + 1.0) / 2.0) * 255.0).round().astype(np.uint8)
|
|
|
|
|
|
def _recv_exact(sock, n):
|
|
"""Read exactly n bytes from a TCP socket (recv() can return short
|
|
reads/chunks at any point - never assume one call gives you n)."""
|
|
buf = b""
|
|
while len(buf) < n:
|
|
chunk = sock.recv(n - len(buf))
|
|
if not chunk:
|
|
raise IOError("media relay connection closed")
|
|
buf += chunk
|
|
return buf
|
|
|
|
|
|
def _send_framed(sock, msg_type, payload):
|
|
"""Mirror of nao_video_server.py's _send() - used to push TYPE_TALK
|
|
(push-to-talk audio) the other way, client -> robot."""
|
|
sock.sendall(struct.pack(">BI", msg_type, len(payload)))
|
|
sock.sendall(payload)
|
|
|
|
|
|
# ==========================================
|
|
# NAO MIC STREAM
|
|
# ==========================================
|
|
class SoundReceiver:
|
|
PREBUFFER_CHUNKS = 1 # was 3 - less silence before playback starts
|
|
TARGET_QUEUE_DEPTH = 4 # once a stall lets the backlog build past
|
|
# QUEUE_MAXSIZE and we start dropping, trim
|
|
# it back down to this instead of just
|
|
# shaving one chunk at a time - a played-back
|
|
# backlog sounds like the mic sped up, and
|
|
# that's more noticeable than a couple of
|
|
# dropped chunks
|
|
QUEUE_MAXSIZE = 12 # was 40 - bounds worst-case playback lag
|
|
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=1024)
|
|
self._play_test_tone()
|
|
self.queue = queue.Queue(maxsize=self.QUEUE_MAXSIZE)
|
|
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):
|
|
self.feed(bytes(buffer))
|
|
|
|
def feed(self, raw_pcm_bytes):
|
|
"""Push one chunk of 16-bit PCM into the playback queue,
|
|
dropping the oldest chunk on overflow instead of blocking.
|
|
Used both by the direct qi audio callback (processRemote,
|
|
above) and by the TCP media relay path (which mu-law-decodes
|
|
back to PCM before calling this)."""
|
|
if not HAS_AUDIO: return
|
|
self.chunks_received += 1
|
|
try:
|
|
self.queue.put_nowait(raw_pcm_bytes)
|
|
except queue.Full:
|
|
try:
|
|
self.queue.get_nowait()
|
|
self.queue.put_nowait(raw_pcm_bytes)
|
|
except: pass
|
|
# A stall (network hiccup, etc.) can let the backlog build up
|
|
# even before the queue is literally Full - trim it back down
|
|
# toward real-time here rather than only reacting once it hits
|
|
# the ceiling above, so a brief stall degrades to a couple of
|
|
# dropped chunks instead of several seconds of played-back lag.
|
|
while self.queue.qsize() > self.TARGET_QUEUE_DEPTH:
|
|
try:
|
|
self.queue.get_nowait()
|
|
except queue.Empty:
|
|
break
|
|
|
|
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
|
|
|
|
# ==========================================
|
|
# PUSH-TO-TALK (over the media relay's audio connection)
|
|
# ==========================================
|
|
class PushToTalk:
|
|
"""
|
|
Push-to-talk: streams the local mic to NAO's speaker over the same
|
|
TCP audio connection the media relay already uses for the robot's
|
|
own mic (see NaoTeleop._audio_relay_loop / _send_talk_chunk) -
|
|
mu-law encoded, framed as TYPE_TALK, decoded and played by
|
|
nao_video_server.py's TalkPlayer.
|
|
|
|
This replaces the old SSH-based relay (`nc -l | aplay` over a
|
|
dedicated SSH channel): no SSH connection of its own, no per-press
|
|
socket handshake - start()/stop() just toggle a background thread
|
|
that reads the mic and hands chunks to send_fn, which writes
|
|
straight into the already-open, already-connected audio socket.
|
|
"""
|
|
def __init__(self, send_fn, is_ready_fn, rate=16000, chunk_frames=1024,
|
|
input_device_index=None, pyaudio_instance=None):
|
|
self.send_fn = send_fn
|
|
self.is_ready_fn = is_ready_fn
|
|
self.rate = rate
|
|
self.chunk_frames = chunk_frames
|
|
self.input_device_index = input_device_index
|
|
# 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)
|
|
|
|
self.active = False
|
|
# "idle" | "live" - read by the HUD to show status
|
|
self.state = "idle"
|
|
self._stream = None
|
|
self._thread = None
|
|
|
|
def start(self):
|
|
if self.active:
|
|
return
|
|
if not HAS_AUDIO:
|
|
print("🎙️ Live mic needs pyaudio installed")
|
|
return
|
|
if not self.is_ready_fn():
|
|
print("🎙️ Talk relay: audio connection not ready yet")
|
|
return
|
|
# If a previous relay thread is still tearing itself down, wait for
|
|
# it to finish before reusing self._stream for a new one.
|
|
if self._thread and self._thread.is_alive():
|
|
self._thread.join(timeout=2.0)
|
|
self.active = True
|
|
self.state = "live"
|
|
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 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() closes it, from its own thread.
|
|
"""
|
|
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 _run(self):
|
|
try:
|
|
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=self.chunk_frames)
|
|
print("🎙️ Talk relay: live")
|
|
while self.active:
|
|
try:
|
|
chunk = self._stream.read(self.chunk_frames, exception_on_overflow=False)
|
|
pcm = np.frombuffer(chunk, dtype=np.int16)
|
|
self.send_fn(encode_ulaw(pcm).tobytes())
|
|
except Exception:
|
|
break
|
|
except Exception as e:
|
|
print(f"🎙️ Talk relay error: {e}")
|
|
finally:
|
|
self.active = False
|
|
self.state = "idle"
|
|
if self._stream:
|
|
try:
|
|
self._stream.stop_stream()
|
|
self._stream.close()
|
|
except: pass
|
|
self._stream = None
|
|
print("🎙️ Talk relay: stopped")
|
|
|
|
def close(self):
|
|
"""No persistent connection of its own to tear down anymore -
|
|
kept as an alias for stop() so shutdown code doesn't need to
|
|
know which push-to-talk implementation it's holding."""
|
|
self.stop()
|
|
|
|
|
|
# ==========================================
|
|
# ON-ROBOT VIDEO/AUDIO RELAY SERVER (auto-deploy)
|
|
# ==========================================
|
|
class VideoServerLauncher:
|
|
"""
|
|
SCPs nao_video_server.py to the robot's home directory over SFTP and
|
|
launches it via a persistent, pty-backed SSH session - so the user
|
|
never has to manually copy the file over or SSH in to start/stop it.
|
|
|
|
Uses the same trick as LiveMicRelay: a pty ties the remote process
|
|
group to the SSH channel, so close() (killing the channel) reliably
|
|
kills the video server too instead of leaving it orphaned on the
|
|
robot, still holding VIDEO_PORT/AUDIO_PORT, for next time.
|
|
"""
|
|
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22,
|
|
local_path=None, remote_filename="nao_video_server.py"):
|
|
self.nao_ip = nao_ip
|
|
self.ssh_user = ssh_user
|
|
self.ssh_password = ssh_password
|
|
self.ssh_port = ssh_port
|
|
self.local_path = local_path or os.path.join(
|
|
os.path.dirname(os.path.abspath(__file__)), remote_filename)
|
|
self.remote_filename = remote_filename
|
|
self.ready = False
|
|
self._ssh_client = None
|
|
self._channel = None
|
|
self._drain_thread = None
|
|
|
|
def deploy_and_start(self):
|
|
if not HAS_SSH:
|
|
print("🎥 Video server auto-deploy needs paramiko: pip install paramiko")
|
|
return False
|
|
if not os.path.isfile(self.local_path):
|
|
print(f"🎥 Video server auto-deploy: can't find {self.local_path}")
|
|
return False
|
|
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=10)
|
|
|
|
# Best-effort: a previous session that didn't get to call
|
|
# close() cleanly (laptop lost network, crashed, etc.) can
|
|
# leave a stale instance squatting on the ports - clear it
|
|
# before starting a fresh one.
|
|
try:
|
|
_, out, _ = self._ssh_client.exec_command(
|
|
f"pkill -f {self.remote_filename}", timeout=5)
|
|
out.channel.recv_exit_status()
|
|
time.sleep(0.3)
|
|
except Exception:
|
|
pass
|
|
|
|
sftp = self._ssh_client.open_sftp()
|
|
try:
|
|
remote_home = sftp.normalize(".") # robot's home dir
|
|
remote_path = remote_home + "/" + self.remote_filename
|
|
print(f"🎥 Copying {self.remote_filename} -> {self.nao_ip}:{remote_path}")
|
|
sftp.put(self.local_path, remote_path)
|
|
finally:
|
|
sftp.close()
|
|
|
|
self._channel = self._ssh_client.get_transport().open_session()
|
|
self._channel.get_pty()
|
|
remote_cmd = (
|
|
"export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages && "
|
|
"export LD_LIBRARY_PATH=/opt/aldebaran/lib && "
|
|
f"python2 {remote_path}"
|
|
)
|
|
self._channel.exec_command(remote_cmd)
|
|
|
|
self._drain_thread = threading.Thread(target=self._drain_output, daemon=True)
|
|
self._drain_thread.start()
|
|
|
|
time.sleep(1.0) # give it a moment to bind before the client tries to connect
|
|
if self._channel.exit_status_ready():
|
|
print("🎥 Video server exited immediately - check the log lines above")
|
|
self.ready = False
|
|
return False
|
|
|
|
self.ready = True
|
|
print("🎥 Video server deployed and running on robot")
|
|
return True
|
|
except Exception as e:
|
|
print(f"🎥 Video server auto-deploy failed: {e}")
|
|
self.ready = False
|
|
return False
|
|
|
|
def _drain_output(self):
|
|
"""Keeps reading the remote process's stdout/stderr so its pty
|
|
buffer never fills up and stalls the server, and so its own
|
|
status prints (quality tier changes, client connects) surface
|
|
locally too."""
|
|
chan = self._channel
|
|
while chan and not chan.closed:
|
|
try:
|
|
got = False
|
|
if chan.recv_ready():
|
|
data = chan.recv(4096)
|
|
if data:
|
|
print(f"🎥 [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}")
|
|
got = True
|
|
if chan.recv_stderr_ready():
|
|
data = chan.recv_stderr(4096)
|
|
if data:
|
|
print(f"🎥 [nao_video_server] {data.decode('utf-8', 'replace').rstrip()}")
|
|
got = True
|
|
if not got:
|
|
time.sleep(0.1)
|
|
except Exception:
|
|
break
|
|
|
|
def close(self):
|
|
"""Kill the pty-backed channel (which kills the remote python
|
|
process with it) and tear down the SSH connection. Call once at
|
|
app shutdown."""
|
|
if self._channel:
|
|
try: self._channel.close()
|
|
except: pass
|
|
self._channel = None
|
|
if self._ssh_client:
|
|
try: self._ssh_client.close()
|
|
except: pass
|
|
self._ssh_client = None
|
|
self.ready = False
|
|
|
|
|
|
# ==========================================
|
|
# CHOREGRAPHE BEHAVIOR DEPLOY + RUN (for gestures)
|
|
# ==========================================
|
|
class ChoregrapheBehaviorRunner:
|
|
"""Deploys a Choregraphe-exported behavior folder (a manifest.xml
|
|
plus one or more '<BehaviorName>/behavior.xar' dirs, i.e. exactly
|
|
what "Export as package" produces before you zip it) to the robot
|
|
over SFTP, installs it via the PackageManager service, and runs it
|
|
via ALBehaviorManager - the same three steps Choregraphe's own
|
|
"Connect to robot" + Play button does, done here in code so a
|
|
gesture script can just point at a local project folder.
|
|
|
|
Caches per local folder for the lifetime of this object: once a
|
|
package has been deployed+installed this session, repeat runs
|
|
just call runBehavior() again instead of re-uploading.
|
|
|
|
NOTE: PackageManager.install() is documented as "install a package
|
|
from a path" without saying whether that path can be a raw folder
|
|
or must be a zipped .pkg - this tries the raw folder first (since
|
|
that's genuinely what Choregraphe's live "current project" push
|
|
to a connected robot does) and falls back to zipping it into a
|
|
.pkg if the robot's PackageManager rejects the folder.
|
|
"""
|
|
def __init__(self, session, nao_ip, ssh_user, ssh_password, ssh_port):
|
|
self.nao_ip = nao_ip
|
|
self.ssh_user = ssh_user
|
|
self.ssh_password = ssh_password
|
|
self.ssh_port = ssh_port
|
|
try:
|
|
self.package_mgr = session.service("PackageManager")
|
|
except Exception:
|
|
self.package_mgr = session.service("ALPackageManager")
|
|
self.behavior_mgr = session.service("ALBehaviorManager")
|
|
self._installed = {} # local_dir -> resolved "<package>/<behavior>" name
|
|
|
|
def _sftp_put_dir(self, sftp, local_dir, remote_dir):
|
|
"""Recursively mirror local_dir onto remote_dir over an
|
|
already-open SFTP session - paramiko has no built-in recursive
|
|
put, only single-file put()."""
|
|
try:
|
|
sftp.mkdir(remote_dir)
|
|
except IOError:
|
|
pass # already exists, fine
|
|
for entry in sorted(os.listdir(local_dir)):
|
|
local_path = os.path.join(local_dir, entry)
|
|
remote_path = remote_dir + "/" + entry
|
|
if os.path.isdir(local_path):
|
|
self._sftp_put_dir(sftp, local_path, remote_path)
|
|
else:
|
|
sftp.put(local_path, remote_path)
|
|
|
|
def ensure_running(self, local_dir, behavior_subdir):
|
|
"""Deploy+install local_dir (only if not already done this
|
|
session), then block until '<package>/<behavior_subdir>' has
|
|
finished running on the robot. local_dir's own folder name
|
|
becomes the package id, matching how Choregraphe names an
|
|
exported project after its folder."""
|
|
if not HAS_SSH:
|
|
raise RuntimeError("Choregraphe gesture deploy needs paramiko: pip install paramiko")
|
|
if not os.path.isdir(local_dir):
|
|
raise RuntimeError(f"No Choregraphe project folder at {local_dir}")
|
|
|
|
package_name = os.path.basename(os.path.normpath(local_dir))
|
|
behavior_name = self._installed.get(local_dir)
|
|
|
|
if behavior_name is None:
|
|
ssh = paramiko.SSHClient()
|
|
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
|
ssh.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
|
|
password=self.ssh_password, timeout=10)
|
|
try:
|
|
sftp = ssh.open_sftp()
|
|
try:
|
|
remote_home = sftp.normalize(".")
|
|
remote_dir = f"{remote_home}/naowalk_gestures/{package_name}"
|
|
print(f"🕺 Copying {package_name} -> {self.nao_ip}:{remote_dir}")
|
|
self._sftp_put_dir(sftp, local_dir, remote_dir)
|
|
|
|
print(f"🕺 Installing package {package_name}...")
|
|
try:
|
|
self.package_mgr.install(remote_dir)
|
|
except Exception as e:
|
|
print(f"🕺 install() on the raw folder failed ({e}) - "
|
|
f"retrying as a zipped .pkg...")
|
|
local_zip = shutil.make_archive(
|
|
os.path.join(tempfile.gettempdir(), package_name), "zip", local_dir)
|
|
remote_zip = f"{remote_home}/naowalk_gestures/{package_name}.pkg"
|
|
sftp.put(local_zip, remote_zip)
|
|
os.remove(local_zip)
|
|
self.package_mgr.install(remote_zip)
|
|
finally:
|
|
sftp.close()
|
|
finally:
|
|
ssh.close()
|
|
|
|
behavior_name = f"{package_name}/{behavior_subdir}"
|
|
if not self.behavior_mgr.isBehaviorInstalled(behavior_name):
|
|
installed = self.behavior_mgr.getInstalledBehaviors()
|
|
candidates = [b for b in installed if b.endswith("/" + behavior_subdir)]
|
|
if not candidates:
|
|
raise RuntimeError(
|
|
f"'{behavior_name}' not found after install - installed behaviors: {installed}")
|
|
behavior_name = candidates[0]
|
|
self._installed[local_dir] = behavior_name
|
|
|
|
print(f"🕺 Running {behavior_name}...")
|
|
self.behavior_mgr.runBehavior(behavior_name) # blocks until it finishes
|
|
|
|
|
|
# ==========================================
|
|
# MAIN TELEOP
|
|
# ==========================================
|
|
class NaoTeleop:
|
|
HEAD_RATE = 1.4 # rad/sec - how fast a held IJKL key sweeps the head target angle
|
|
|
|
def __init__(self, session, nao_ip, audio_device_index=None, mic_gain=8.0, video_scale=2.0,
|
|
nao_ssh_user="nao", nao_ssh_password="nao", nao_ssh_port=22, relay_rate=16000,
|
|
media_relay=None, deploy_video_server=True, video_server_path=None,
|
|
gestures_dir=None):
|
|
self.session = session
|
|
self.nao_ip = nao_ip
|
|
self.nao_ssh_user = nao_ssh_user
|
|
self.nao_ssh_password = nao_ssh_password
|
|
self.nao_ssh_port = nao_ssh_port
|
|
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 # only used for volume get/set now - video
|
|
# and both mic directions are relay-only
|
|
self.audio_device_index = audio_device_index
|
|
self.mic_gain = mic_gain
|
|
|
|
# One shared PyAudio context for the whole app - SoundReceiver (mic
|
|
# playback) and PushToTalk (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
|
|
|
|
# The audio relay connection is bidirectional (see
|
|
# _audio_relay_loop) - this is the socket PushToTalk writes
|
|
# TYPE_TALK chunks into, guarded by a lock since the relay loop
|
|
# (on its own thread) can tear it down and reconnect at any time.
|
|
self._audio_sock = None
|
|
self._audio_sock_lock = threading.Lock()
|
|
|
|
self.mic_relay = PushToTalk(send_fn=self._send_talk_chunk, is_ready_fn=self._audio_relay_ready,
|
|
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._last_head_update = time.time()
|
|
self.battery_level = 100
|
|
self.last_batt_check = 0
|
|
self.volume = 50
|
|
self.walk_speed = 1.0 # fixed, not adjustable at runtime - 0.60 was too slow
|
|
# for the walk engine to stay balanced and NAO kept
|
|
# tipping over
|
|
|
|
self.typing_mode = False
|
|
self.is_resting = False # tracks manual [DEL] crouch, toggled by idle_crouch()
|
|
self.chat_message = ""
|
|
self.music_search_mode = False
|
|
self.music_search_text = ""
|
|
self.music_search_pending = False
|
|
|
|
self.video_scale = video_scale
|
|
self.media_relay = media_relay
|
|
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 / 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
|
|
|
|
# Deploy+launch nao_video_server.py over SSH now (before _init_video
|
|
# tries to connect to it) instead of requiring it to already be
|
|
# running manually. Everything - video, robot mic, push-to-talk -
|
|
# goes through this relay; there's no direct-qi fallback anymore.
|
|
self.video_server_launcher = None
|
|
if deploy_video_server:
|
|
self.video_server_launcher = VideoServerLauncher(
|
|
nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password,
|
|
ssh_port=nao_ssh_port, local_path=video_server_path)
|
|
self.video_server_launcher.deploy_and_start()
|
|
|
|
self._init_audio_device()
|
|
self._init_video()
|
|
if HAS_AUDIO:
|
|
self._init_mic_stream()
|
|
|
|
# Lets gesture scripts run full Choregraphe-authored behaviors
|
|
# (.xar, exported as a package folder) instead of hand-coding
|
|
# angleInterpolation calls - see ChoregrapheBehaviorRunner.
|
|
self.behavior_runner = ChoregrapheBehaviorRunner(
|
|
session, nao_ip, nao_ssh_user, nao_ssh_password, nao_ssh_port)
|
|
|
|
self.gestures_dir = gestures_dir or os.path.join(
|
|
os.path.dirname(os.path.abspath(__file__)), "gestures")
|
|
self._load_gestures()
|
|
|
|
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(self):
|
|
print(f"✅ Camera via on-robot media relay: {self.media_relay}")
|
|
threading.Thread(target=self._video_relay_loop, daemon=True).start()
|
|
|
|
def _video_relay_loop(self):
|
|
"""Runs on its own thread for the app's lifetime, speaking the
|
|
lean length-framed TCP protocol from nao_video_server.py. The
|
|
render loop never touches the socket directly; it only ever
|
|
reads whatever self._latest_frame was last successfully set
|
|
to, so a stalled/reconnecting link can no longer freeze
|
|
keyboard input or movement. Video-only connection - a slow
|
|
video send could otherwise block audio behind it, so they're
|
|
split (see the audio port, media_relay port + 1, handled by
|
|
_audio_relay_loop)."""
|
|
host, _, port_str = self.media_relay.partition(":")
|
|
port = int(port_str) if port_str else 8000
|
|
while not self._video_stop_event.is_set():
|
|
sock = None
|
|
try:
|
|
sock = socket.create_connection((host, port), timeout=5)
|
|
sock.settimeout(5.0)
|
|
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
|
print(f"✅ Video relay connected: {host}:{port}")
|
|
while not self._video_stop_event.is_set():
|
|
msg_type, length = struct.unpack(">BI", _recv_exact(sock, 5))
|
|
payload = _recv_exact(sock, length) if length else b""
|
|
if msg_type == TYPE_VIDEO:
|
|
arr = np.frombuffer(payload, 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()
|
|
# TYPE_HELLO: nothing to do with it yet, just skip.
|
|
except Exception as e:
|
|
print(f"video relay error: {e}")
|
|
finally:
|
|
if sock:
|
|
try: sock.close()
|
|
except: pass
|
|
if not self._video_stop_event.is_set():
|
|
time.sleep(0.5)
|
|
|
|
def _audio_relay_loop(self):
|
|
"""Mic audio's own connection - see _video_relay_loop for why
|
|
this is split off rather than sharing video's socket. Connects
|
|
to media_relay's host:port+1 by convention (matches AUDIO_PORT
|
|
= VIDEO_PORT + 1 in nao_video_server.py).
|
|
|
|
This connection is bidirectional: this loop reads TYPE_AUDIO
|
|
(robot's mic) off it, and _send_talk_chunk (called from the
|
|
PushToTalk thread) writes TYPE_TALK (push-to-talk audio) onto
|
|
the same socket - TCP is full-duplex, so neither direction
|
|
blocks the other. self._audio_sock is published here (and
|
|
cleared on disconnect) so _send_talk_chunk always has whatever
|
|
socket is currently live, or nothing to send to while
|
|
reconnecting."""
|
|
host, _, port_str = self.media_relay.partition(":")
|
|
video_port = int(port_str) if port_str else 8000
|
|
port = video_port + 1
|
|
while not self._video_stop_event.is_set():
|
|
sock = None
|
|
try:
|
|
sock = socket.create_connection((host, port), timeout=5)
|
|
sock.settimeout(5.0)
|
|
sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
|
with self._audio_sock_lock:
|
|
self._audio_sock = sock
|
|
print(f"✅ Audio relay connected: {host}:{port}")
|
|
while not self._video_stop_event.is_set():
|
|
msg_type, length = struct.unpack(">BI", _recv_exact(sock, 5))
|
|
payload = _recv_exact(sock, length) if length else b""
|
|
if msg_type == TYPE_AUDIO and getattr(self, "sound_receiver", None):
|
|
pcm = decode_ulaw(np.frombuffer(payload, dtype=np.uint8))
|
|
self.sound_receiver.feed(pcm.tobytes())
|
|
# TYPE_HELLO: nothing to do with it yet, just skip.
|
|
except Exception as e:
|
|
print(f"audio relay error: {e}")
|
|
finally:
|
|
with self._audio_sock_lock:
|
|
if self._audio_sock is sock:
|
|
self._audio_sock = None
|
|
if sock:
|
|
try: sock.close()
|
|
except: pass
|
|
if not self._video_stop_event.is_set():
|
|
time.sleep(0.5)
|
|
|
|
def _audio_relay_ready(self):
|
|
"""Whether the audio connection is currently up - PushToTalk
|
|
checks this before starting a press so it can fail fast with a
|
|
clear message instead of silently sending into the void."""
|
|
with self._audio_sock_lock:
|
|
return self._audio_sock is not None
|
|
|
|
def _send_talk_chunk(self, payload):
|
|
"""Called from the PushToTalk thread with one already mu-law-
|
|
encoded chunk. Grabs whatever socket _audio_relay_loop currently
|
|
has published and writes straight into it - if the link drops
|
|
mid-send this just raises/gets swallowed and the chunk is lost,
|
|
which is fine for live voice (the next chunk is a beat away)."""
|
|
with self._audio_sock_lock:
|
|
sock = self._audio_sock
|
|
if not sock:
|
|
return
|
|
try:
|
|
_send_framed(sock, TYPE_TALK, payload)
|
|
except Exception:
|
|
pass # _audio_relay_loop will notice the failure and reconnect
|
|
|
|
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):
|
|
try:
|
|
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
|
pyaudio_instance=self._pyaudio)
|
|
print(f"✅ Mic stream ready (gain={self.mic_gain}x, via media relay - compressed, own connection)")
|
|
threading.Thread(target=self._audio_relay_loop, daemon=True).start()
|
|
except Exception as e:
|
|
print(f"❌ Mic init failed: {e}")
|
|
|
|
def _load_gestures(self):
|
|
"""Load number-key gestures from .py files in self.gestures_dir.
|
|
|
|
Each file is named '<digit>_<name>.py' (e.g. '4_gangnam_style.py')
|
|
and must define a module-level run(motion, tts) function - that's
|
|
the minimum contract, gets the already-connected ALMotion and
|
|
ALTextToSpeech proxies and does whatever it wants with them.
|
|
Gestures that need more - e.g. running a Choregraphe behavior via
|
|
self.behavior_runner, or resetting posture via
|
|
self.stand_for_walk() - can instead define run(motion, tts,
|
|
teleop) and get this NaoTeleop instance as a third argument; the
|
|
arity is detected here at load time so both signatures work. The
|
|
leading digit is which number key (0-9) triggers it; everything
|
|
after the underscore becomes its display name in the controls
|
|
hint, unless the file sets its own module-level NAME string.
|
|
"""
|
|
self.gestures = {} # pygame key constant -> (display_name, run_fn, n_params)
|
|
gestures_dir = self.gestures_dir
|
|
if not os.path.isdir(gestures_dir):
|
|
print(f"⚠️ No gestures folder at {gestures_dir}, skipping gesture load")
|
|
return
|
|
|
|
for fname in sorted(os.listdir(gestures_dir)):
|
|
if not fname.endswith(".py") or fname.startswith("_"):
|
|
continue
|
|
stem = fname[:-3]
|
|
digit_str, sep, rest = stem.partition("_")
|
|
if not (sep and len(digit_str) == 1 and digit_str.isdigit()):
|
|
print(f"⚠️ Skipping {fname}: name it '<digit>_name.py' (e.g. '4_gangnam_style.py')")
|
|
continue
|
|
|
|
path = os.path.join(gestures_dir, fname)
|
|
try:
|
|
spec = importlib.util.spec_from_file_location(f"nao_gesture_{stem}", path)
|
|
module = importlib.util.module_from_spec(spec)
|
|
spec.loader.exec_module(module)
|
|
run_fn = getattr(module, "run", None)
|
|
if not callable(run_fn):
|
|
print(f"⚠️ Skipping {fname}: no run(motion, tts) function defined")
|
|
continue
|
|
try:
|
|
n_params = len(inspect.signature(run_fn).parameters)
|
|
except (TypeError, ValueError):
|
|
n_params = 2
|
|
except Exception as e:
|
|
print(f"⚠️ Failed to load gesture {fname}: {e}")
|
|
continue
|
|
|
|
key = getattr(pygame, f"K_{digit_str}")
|
|
name = getattr(module, "NAME", rest.replace("_", " ").title() or stem)
|
|
if key in self.gestures:
|
|
print(f"⚠️ {fname} overrides number key {digit_str} (was {self.gestures[key][0]})")
|
|
self.gestures[key] = (name, run_fn, n_params)
|
|
print(f"✅ Loaded gesture: {digit_str}={name}")
|
|
|
|
def run_gesture(self, key):
|
|
if self.typing_mode or self.is_resting: return
|
|
entry = self.gestures.get(key)
|
|
if not entry: return
|
|
name, run_fn, n_params = entry
|
|
print(f"🕺 {name}...")
|
|
try:
|
|
if n_params >= 3:
|
|
run_fn(self.motion, self.tts, self)
|
|
else:
|
|
run_fn(self.motion, self.tts)
|
|
except Exception as e:
|
|
print(f"{name} gesture error: {e}")
|
|
|
|
def stand_for_walk(self):
|
|
"""Re-stiffen and drop straight into walk-ready posture -
|
|
mirrors the startup sequence in run() (stiffen -> walk arms ->
|
|
relax arm stiffness -> moveInit) rather than
|
|
goToPosture("Stand"), which goes through its own separate
|
|
stand-up animation instead of just being walk-ready. Shared by
|
|
the [DEL] wake-up toggle and by any gesture that needs to
|
|
reset his stance afterward (e.g. after a Choregraphe behavior
|
|
that moved the legs/hips)."""
|
|
self._safe("Restiffen", lambda: self.motion.setStiffnesses("Body", 1.0))
|
|
self._safe("Walk arms", lambda: self.motion.setMoveArmsEnabled(True, True))
|
|
self._safe("Relax arms", lambda: self.motion.setStiffnesses(["RArm", "LArm"], 0.3))
|
|
self._safe("moveInit", self.motion.moveInit)
|
|
self.is_resting = False
|
|
|
|
def idle_crouch(self):
|
|
"""The relaxed sit/crouch posture NAO settles into on shutdown -
|
|
toggled on demand with the Delete key. rest() cuts stiffness to
|
|
the whole body (that's the point of "rest"), so simply calling
|
|
it again does nothing on the second press - we have to
|
|
explicitly re-stiffen ourselves via stand_for_walk()."""
|
|
if not self.is_resting:
|
|
print("😴 Resting (idle crouch)...")
|
|
self._safe("stopMove", self.motion.stopMove)
|
|
self._safe("rest", self.motion.rest)
|
|
self.is_resting = True
|
|
else:
|
|
print("🧍 Waking up...")
|
|
self.stand_for_walk()
|
|
|
|
def handle_head_movement(self):
|
|
if self.typing_mode or self.music_search_mode: return
|
|
keys = pygame.key.get_pressed()
|
|
|
|
now = time.time()
|
|
dt = now - self._last_head_update
|
|
self._last_head_update = now
|
|
step = self.HEAD_RATE * dt
|
|
|
|
changed = False
|
|
if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - step, -0.5); changed = True
|
|
if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + step, 0.5); changed = True
|
|
if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + step, 2.0); changed = True
|
|
if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - step, -2.0); changed = True
|
|
|
|
if changed:
|
|
self._safe("Head move", lambda: self.motion.setAngles(
|
|
["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.18))
|
|
|
|
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")
|
|
|
|
self._movement_thread = threading.Thread(target=self._movement_loop, daemon=True)
|
|
self._movement_thread.start()
|
|
|
|
gesture_hint = " | ".join(
|
|
f"{pygame.key.name(key)}={name}" for key, (name, *_rest) in sorted(self.gestures.items())
|
|
)
|
|
print(f"\n🎮 Controls: WASD/Arrows=Walk | IJKL=Head | SPACE=Reset head | {gesture_hint} | "
|
|
f"DEL=Toggle crouch/stand | 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_DELETE:
|
|
self.idle_crouch()
|
|
elif event.key in self.gestures:
|
|
self.run_gesture(event.key)
|
|
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
|
|
|
|
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 getattr(self, "video_server_launcher", None):
|
|
self._safe("video server stop", self.video_server_launcher.close)
|
|
if getattr(self, "sound_receiver", None):
|
|
self._safe("sound receiver close", self.sound_receiver.close)
|
|
if getattr(self, "_pyaudio", None):
|
|
self._safe("pyaudio terminate", self._pyaudio.terminate)
|
|
self._safe("rest", self.motion.rest)
|
|
pygame.quit()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
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-gain", type=float, default=8.0)
|
|
parser.add_argument("--video-scale", type=float, default=2.0)
|
|
parser.add_argument("--media-relay", type=str, default=None,
|
|
help="host:port to reach the on-robot media relay (nao_video_server.py) "
|
|
"at, e.g. 127.0.0.1:8000 when tunneled through the phone. Setting "
|
|
"this also makes naowalk auto-scp nao_video_server.py to the "
|
|
"robot's home dir over SSH and launch it (see --no-deploy-video-"
|
|
"server to skip that and connect to one you started yourself). "
|
|
"BOTH video and mic audio come compressed over this one TCP "
|
|
"connection (JPEG + mu-law) instead of raw frames/PCM over the "
|
|
"qi session. To change the mic channel, set MIC_CHANNEL in "
|
|
"nao_video_server.py, since the mic subscription lives there now.")
|
|
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-rate", type=int, default=16000,
|
|
help="Sample rate for the live mic relay - try 48000 if audio sounds sped up/slow")
|
|
parser.add_argument("--no-deploy-video-server", action="store_true",
|
|
help="Don't auto-scp/launch nao_video_server.py on the robot when "
|
|
"--media-relay is set - use this if you're already running it "
|
|
"yourself and just want naowalk to connect to it.")
|
|
parser.add_argument("--video-server-path", type=str, default=None,
|
|
help="Local path to nao_video_server.py to deploy (default: the "
|
|
"copy sitting next to naowalk.py)")
|
|
parser.add_argument("--gestures-dir", type=str, default=None,
|
|
help="Folder of number-key gestures to load, each file named "
|
|
"'<digit>_name.py' with a run(motion, tts) function "
|
|
"(default: the 'gestures' folder next to naowalk.py)")
|
|
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_gain=args.mic_gain,
|
|
video_scale=args.video_scale,
|
|
media_relay=args.media_relay,
|
|
nao_ssh_user=args.nao_ssh_user,
|
|
nao_ssh_password=args.nao_ssh_password,
|
|
nao_ssh_port=args.nao_ssh_port,
|
|
relay_rate=args.relay_rate,
|
|
deploy_video_server=not args.no_deploy_video_server,
|
|
video_server_path=args.video_server_path,
|
|
gestures_dir=args.gestures_dir).run()
|