setup compression using the relay server for back and forth audio communication
This commit is contained in:
+170
-214
@@ -35,13 +35,18 @@ except ImportError:
|
||||
|
||||
# ==========================================
|
||||
# MEDIA RELAY WIRE PROTOCOL (see nao_video_server.py)
|
||||
# One TCP connection, length-framed: [1 byte type][4 byte length][payload].
|
||||
# Replaces the old HTTP MJPEG pull - no multipart parsing, and now
|
||||
# carries mu-law-compressed mic audio too, not just video.
|
||||
# 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):
|
||||
@@ -53,6 +58,18 @@ def decode_ulaw(q):
|
||||
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)."""
|
||||
@@ -65,11 +82,26 @@ def _recv_exact(sock, n):
|
||||
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 = 3
|
||||
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):
|
||||
@@ -99,9 +131,9 @@ class SoundReceiver:
|
||||
|
||||
self.stream = self.p.open(format=pyaudio.paInt16, channels=1, rate=16000,
|
||||
output=True, output_device_index=output_device_index,
|
||||
frames_per_buffer=2048)
|
||||
frames_per_buffer=1024)
|
||||
self._play_test_tone()
|
||||
self.queue = queue.Queue(maxsize=40)
|
||||
self.queue = queue.Queue(maxsize=self.QUEUE_MAXSIZE)
|
||||
self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True)
|
||||
self.playback_thread.start()
|
||||
|
||||
@@ -131,6 +163,16 @@ class SoundReceiver:
|
||||
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 = []
|
||||
@@ -187,90 +229,39 @@ class SoundReceiver:
|
||||
except: pass
|
||||
|
||||
# ==========================================
|
||||
# LIVE MIC RELAY (push-to-talk over SSH)
|
||||
# PUSH-TO-TALK (over the media relay's audio connection)
|
||||
# ==========================================
|
||||
class LiveMicRelay:
|
||||
class PushToTalk:
|
||||
"""
|
||||
Push-to-talk: streams the local mic straight to NAO's own speaker.
|
||||
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.
|
||||
|
||||
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.
|
||||
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, 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
|
||||
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
|
||||
|
||||
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
|
||||
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:
|
||||
@@ -278,65 +269,44 @@ class LiveMicRelay:
|
||||
if not HAS_AUDIO:
|
||||
print("🎙️ Live mic needs pyaudio installed")
|
||||
return
|
||||
if not self.connected:
|
||||
print("🎙️ Talk relay: remote listener not ready yet")
|
||||
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/_sock for a new one.
|
||||
# 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 = "connecting"
|
||||
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/_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.
|
||||
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 _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)
|
||||
frames_per_buffer=self.chunk_frames)
|
||||
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)
|
||||
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:
|
||||
@@ -344,9 +314,21 @@ class LiveMicRelay:
|
||||
finally:
|
||||
self.active = False
|
||||
self.state = "idle"
|
||||
self._cleanup()
|
||||
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)
|
||||
# ==========================================
|
||||
@@ -478,9 +460,9 @@ class VideoServerLauncher:
|
||||
# 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, media_relay=None, deploy_video_server=True, video_server_path=None):
|
||||
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):
|
||||
self.session = session
|
||||
self.nao_ip = nao_ip
|
||||
self.motion = session.service("ALMotion")
|
||||
@@ -495,20 +477,25 @@ class NaoTeleop:
|
||||
self.music_results = []
|
||||
self.selected_result = 0
|
||||
|
||||
self.audio_device = None
|
||||
self.audio_service_name = "SoundReceiver"
|
||||
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_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,
|
||||
# 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
|
||||
|
||||
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)
|
||||
# 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
|
||||
@@ -526,18 +513,12 @@ class NaoTeleop:
|
||||
self.music_search_pending = False
|
||||
|
||||
self.video_scale = video_scale
|
||||
self.video_fps = video_fps
|
||||
self.media_relay = media_relay
|
||||
# True only if we actually subscribed to ALAudioDevice ourselves
|
||||
# (i.e. not using the relay, where nao_video_server.py owns the
|
||||
# mic subscription instead) - used at shutdown so we don't try
|
||||
# to unsubscribe from something we never subscribed to.
|
||||
self._qi_audio_subscribed = False
|
||||
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
|
||||
# _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
|
||||
@@ -546,19 +527,19 @@ class NaoTeleop:
|
||||
self._movement_stop_event = threading.Event()
|
||||
self._movement_thread = None
|
||||
|
||||
# If we're using the on-robot media relay, deploy+launch
|
||||
# nao_video_server.py over SSH now (before _init_video_maxfps
|
||||
# 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.
|
||||
# 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 self.media_relay and deploy_video_server:
|
||||
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_maxfps()
|
||||
self._init_video()
|
||||
if HAS_AUDIO:
|
||||
self._init_mic_stream()
|
||||
|
||||
@@ -585,48 +566,20 @@ class NaoTeleop:
|
||||
print(f"⚠️ ALAudioDevice error: {e}")
|
||||
self.audio_device = None
|
||||
|
||||
def _init_video_maxfps(self):
|
||||
if self.media_relay:
|
||||
self.video = None
|
||||
self.video_client = "relay" # truthy sentinel, unsubscribe() is a no-op for this path
|
||||
print(f"✅ Camera via on-robot media relay: {self.media_relay}")
|
||||
threading.Thread(target=self._video_relay_loop, 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 _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):
|
||||
"""Same role as _video_loop, but speaks the lean length-framed
|
||||
TCP protocol from nao_video_server.py instead of pulling raw
|
||||
frames over the qi session. Video-only connection - it used to
|
||||
share one socket with audio, but a slow video send could then
|
||||
block audio behind it, so they're split (see the audio port,
|
||||
media_relay port + 1, handled by _audio_relay_loop)."""
|
||||
"""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():
|
||||
@@ -659,7 +612,16 @@ class NaoTeleop:
|
||||
"""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)."""
|
||||
= 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
|
||||
@@ -669,6 +631,8 @@ class NaoTeleop:
|
||||
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))
|
||||
@@ -680,12 +644,37 @@ class NaoTeleop:
|
||||
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
|
||||
@@ -693,23 +682,11 @@ class NaoTeleop:
|
||||
return self._latest_frame
|
||||
|
||||
def _init_mic_stream(self):
|
||||
if not self.audio_device: return
|
||||
try:
|
||||
# Playback machinery is needed either way - only the source
|
||||
# of audio chunks differs (qi pub/sub below, vs mu-law
|
||||
# chunks arriving over the audio relay connection).
|
||||
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
||||
pyaudio_instance=self._pyaudio)
|
||||
if self.media_relay:
|
||||
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()
|
||||
return
|
||||
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)
|
||||
self._qi_audio_subscribed = True
|
||||
print(f"✅ Mic stream ready (gain={self.mic_gain}x, uncompressed direct - use --media-relay on a tunneled link)")
|
||||
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}")
|
||||
|
||||
@@ -883,9 +860,6 @@ class NaoTeleop:
|
||||
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()
|
||||
|
||||
@@ -1137,12 +1111,8 @@ class NaoTeleop:
|
||||
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 self.audio_device and self._qi_audio_subscribed:
|
||||
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)
|
||||
@@ -1150,20 +1120,12 @@ class NaoTeleop:
|
||||
|
||||
|
||||
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 --media-relay is set (the server controls fps).")
|
||||
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 "
|
||||
@@ -1172,9 +1134,8 @@ if __name__ == "__main__":
|
||||
"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 - use this on a tunneled/shoddy link. --mic-channel "
|
||||
"is ignored in this mode (set MIC_CHANNEL in nao_video_server.py "
|
||||
"instead, since the mic subscription lives there now).")
|
||||
"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",
|
||||
@@ -1182,8 +1143,6 @@ if __name__ == "__main__":
|
||||
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")
|
||||
parser.add_argument("--no-deploy-video-server", action="store_true",
|
||||
@@ -1205,15 +1164,12 @@ if __name__ == "__main__":
|
||||
|
||||
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,
|
||||
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_port=args.relay_port,
|
||||
relay_rate=args.relay_rate,
|
||||
deploy_video_server=not args.no_deploy_video_server,
|
||||
video_server_path=args.video_server_path).run()
|
||||
|
||||
Reference in New Issue
Block a user