Compare commits
3 Commits
5977e6fc7d
...
ae9d1652a4
| Author | SHA1 | Date | |
|---|---|---|---|
| ae9d1652a4 | |||
| 7d0b968b36 | |||
| 4a4625028b |
@@ -0,0 +1,4 @@
|
|||||||
|
#!/bin/bash
|
||||||
|
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
||||||
|
#python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
||||||
|
python3.11 naowalk.py --ip 127.0.0.1 --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 2222 --media-relay 127.0.0.1:8000
|
||||||
Binary file not shown.
@@ -0,0 +1,327 @@
|
|||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
"""
|
||||||
|
nao_video_server.py - RUNS ON THE ROBOT, under its own Python 2.7.
|
||||||
|
|
||||||
|
Opens a qi session to NAOqi over localhost (zero network hop - it's
|
||||||
|
the same box) and serves TWO persistent, length-framed TCP streams -
|
||||||
|
one for compressed video (JPEG), one for compressed audio (mu-law) -
|
||||||
|
each on its own port/connection. They used to share one connection,
|
||||||
|
but a single big video frame mid-send could block audio behind it for
|
||||||
|
its whole transfer time, which is a bad tradeoff for something as
|
||||||
|
latency-sensitive as audio. Splitting them means a slow video frame
|
||||||
|
can never delay audio.
|
||||||
|
|
||||||
|
Why not HTTP: the phone tunnel only forwards specific TCP ports, so
|
||||||
|
this still rides over TCP - but a 5-byte binary header has none of
|
||||||
|
HTTP's chunked/multipart-boundary parsing overhead, and it's trivial
|
||||||
|
to keep alive and reconnect on a flaky link.
|
||||||
|
|
||||||
|
Why mic capture lives here now: it used to be shipped raw (16-bit PCM,
|
||||||
|
uncompressed) straight through NAOqi's own pub/sub over the qi RPC
|
||||||
|
session the client holds - i.e. across the tunnel, uncompressed, the
|
||||||
|
whole time. Subscribing to ALAudioDevice locally (same zero-hop trick
|
||||||
|
as the camera) means only mu-law-compressed bytes ever cross the
|
||||||
|
tunnel, half the size of raw PCM.
|
||||||
|
|
||||||
|
Wire format, identical on both connections (all integers big-endian,
|
||||||
|
via `struct`):
|
||||||
|
1 byte type 0x00 HELLO 0x01 VIDEO 0x02 AUDIO
|
||||||
|
4 bytes length
|
||||||
|
N bytes payload
|
||||||
|
HELLO payload: JSON, informational only
|
||||||
|
VIDEO payload: JPEG bytes (video connection only)
|
||||||
|
AUDIO payload: mu-law encoded 8-bit samples, mono (audio connection only)
|
||||||
|
|
||||||
|
Under load (slow/lossy tunnel), video quality and frame rate step
|
||||||
|
down automatically (see QUALITY_TIERS) and recover once sends are
|
||||||
|
fast again - and since encoding is throttled to match, this also cuts
|
||||||
|
CPU use on a congested link instead of just wasting it on frames that
|
||||||
|
would've been discarded anyway.
|
||||||
|
|
||||||
|
Needs the SDK's own env vars, since the bindings aren't on a login
|
||||||
|
shell's default path:
|
||||||
|
|
||||||
|
export PYTHONPATH=/opt/aldebaran/lib/python2.7/site-packages
|
||||||
|
export LD_LIBRARY_PATH=/opt/aldebaran/lib
|
||||||
|
python2 nao_video_server.py
|
||||||
|
|
||||||
|
Stdlib only (BaseHTTPServer/Flask are gone entirely now) plus qi,
|
||||||
|
cv2, and numpy, which are already confirmed present on the robot.
|
||||||
|
"""
|
||||||
|
import collections
|
||||||
|
import json
|
||||||
|
import socket
|
||||||
|
import struct
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
import qi
|
||||||
|
|
||||||
|
# ---- tunables ----
|
||||||
|
NAOQI_PORT = 9561 # local qi session port (confirmed working)
|
||||||
|
VIDEO_PORT = 8000 # forward both of these through the phone tunnel
|
||||||
|
AUDIO_PORT = 8001 # kept on its own connection so a slow video
|
||||||
|
# frame can never block/delay audio delivery
|
||||||
|
CAMERA_RESOLUTION = 1 # 0=kQQVGA(160x120) 1=kQVGA(320x240) 2=kVGA(640x480)
|
||||||
|
CAMERA_COLORSPACE = 11 # kRGBColorSpace
|
||||||
|
CAMERA_FPS = 20
|
||||||
|
AUDIO_RATE = 16000
|
||||||
|
AUDIO_CHANNELS = 1
|
||||||
|
MIC_CHANNEL = 1 # 1=left 2=right 3=front 4=rear (ALAudioDevice channel index)
|
||||||
|
AUDIO_QUEUE_MAX = 60 # chunks buffered before we start dropping the oldest
|
||||||
|
|
||||||
|
# (jpeg_quality, min_seconds_between_frames) - best link first, worst last.
|
||||||
|
# The server steps down a tier when a video send is slow, and back up
|
||||||
|
# after a run of fast ones.
|
||||||
|
QUALITY_TIERS = [
|
||||||
|
(70, 0.05),
|
||||||
|
(55, 0.07),
|
||||||
|
(40, 0.10),
|
||||||
|
(25, 0.15),
|
||||||
|
]
|
||||||
|
SLOW_SEND_THRESHOLD = 0.20 # seconds - a send this slow means congestion
|
||||||
|
FAST_STREAK_TO_RECOVER = 30 # consecutive quick sends before stepping back up
|
||||||
|
|
||||||
|
TYPE_HELLO = 0x00
|
||||||
|
TYPE_VIDEO = 0x01
|
||||||
|
TYPE_AUDIO = 0x02
|
||||||
|
|
||||||
|
|
||||||
|
def encode_ulaw(pcm16):
|
||||||
|
"""int16 numpy array -> uint8 numpy array, mu-law companded.
|
||||||
|
Must match decode_ulaw() in naowalk.py exactly, or the far end
|
||||||
|
just hears noise - keep the two in sync if you touch this."""
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
class FrameGrabber(object):
|
||||||
|
"""Background thread: keeps the single most recent JPEG-encoded
|
||||||
|
frame available, at whatever quality tier the link currently
|
||||||
|
supports. A stalled capture never blocks a client's read."""
|
||||||
|
|
||||||
|
def __init__(self, session):
|
||||||
|
self.video = session.service("ALVideoDevice")
|
||||||
|
self.client = self.video.subscribeCamera(
|
||||||
|
"MediaServer", 0, CAMERA_RESOLUTION, CAMERA_COLORSPACE, CAMERA_FPS)
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._jpeg = None
|
||||||
|
self._tier = 0
|
||||||
|
self._fast_streak = 0
|
||||||
|
self._stop = threading.Event()
|
||||||
|
self._thread = threading.Thread(target=self._loop)
|
||||||
|
self._thread.daemon = True
|
||||||
|
self._thread.start()
|
||||||
|
|
||||||
|
def _loop(self):
|
||||||
|
last_encode = 0.0
|
||||||
|
while not self._stop.is_set():
|
||||||
|
try:
|
||||||
|
image = self.video.getImageRemote(self.client)
|
||||||
|
if not image or len(image) < 7:
|
||||||
|
continue
|
||||||
|
# getImageRemote() blocks until the next camera frame is
|
||||||
|
# ready (~1/CAMERA_FPS), so this loop is already paced -
|
||||||
|
# but under a congested tier we send far less often than
|
||||||
|
# that, so skip the (expensive) convert+encode work for
|
||||||
|
# frames we're just going to overwrite unsent anyway.
|
||||||
|
quality, min_interval = self.quality_and_interval()
|
||||||
|
now = time.time()
|
||||||
|
if now - last_encode < min_interval:
|
||||||
|
continue
|
||||||
|
w, h = image[0], image[1]
|
||||||
|
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
|
||||||
|
bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
||||||
|
ok, buf = cv2.imencode(".jpg", bgr, [cv2.IMWRITE_JPEG_QUALITY, quality])
|
||||||
|
if ok:
|
||||||
|
with self._lock:
|
||||||
|
self._jpeg = buf.tobytes()
|
||||||
|
last_encode = now
|
||||||
|
except Exception as e:
|
||||||
|
print("frame grab error: %s" % e)
|
||||||
|
time.sleep(0.2)
|
||||||
|
|
||||||
|
def quality_and_interval(self):
|
||||||
|
with self._lock:
|
||||||
|
return QUALITY_TIERS[self._tier]
|
||||||
|
|
||||||
|
def report_send_time(self, elapsed):
|
||||||
|
"""Called by the writer thread after each video send so we can
|
||||||
|
adapt to how the link is actually behaving right now."""
|
||||||
|
with self._lock:
|
||||||
|
if elapsed > SLOW_SEND_THRESHOLD:
|
||||||
|
self._fast_streak = 0
|
||||||
|
if self._tier < len(QUALITY_TIERS) - 1:
|
||||||
|
self._tier += 1
|
||||||
|
print("link looks congested - dropping to quality tier %d" % self._tier)
|
||||||
|
else:
|
||||||
|
self._fast_streak += 1
|
||||||
|
if self._fast_streak >= FAST_STREAK_TO_RECOVER and self._tier > 0:
|
||||||
|
self._tier -= 1
|
||||||
|
self._fast_streak = 0
|
||||||
|
print("link recovered - raising to quality tier %d" % self._tier)
|
||||||
|
|
||||||
|
def latest(self):
|
||||||
|
with self._lock:
|
||||||
|
return self._jpeg
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
self._stop.set()
|
||||||
|
try:
|
||||||
|
self.video.unsubscribe(self.client)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
class MicGrabber(object):
|
||||||
|
"""Subscribes to the robot's own mic locally (zero-hop, same trick
|
||||||
|
as FrameGrabber) and mu-law-encodes each chunk as it arrives. This
|
||||||
|
replaces the old design where raw PCM was pushed to the client
|
||||||
|
directly through NAOqi's pub/sub over the (tunneled) qi session."""
|
||||||
|
|
||||||
|
def __init__(self, session, channel=MIC_CHANNEL):
|
||||||
|
self.audio = session.service("ALAudioDevice")
|
||||||
|
self._svc_name = "MicRelay"
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self._chunks = collections.deque(maxlen=AUDIO_QUEUE_MAX)
|
||||||
|
session.registerService(self._svc_name, self)
|
||||||
|
time.sleep(0.5) # let NAOqi's service directory propagate the
|
||||||
|
# registration before ALAudioDevice looks it up
|
||||||
|
# by name, or subscribe() fails to find it
|
||||||
|
self.audio.setClientPreferences(self._svc_name, AUDIO_RATE, channel, 0)
|
||||||
|
self.audio.subscribe(self._svc_name)
|
||||||
|
|
||||||
|
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
||||||
|
try:
|
||||||
|
pcm = np.frombuffer(bytes(buffer), dtype=np.int16)
|
||||||
|
chunk = encode_ulaw(pcm).tobytes()
|
||||||
|
with self._lock:
|
||||||
|
self._chunks.append(chunk)
|
||||||
|
except Exception as e:
|
||||||
|
print("mic encode error: %s" % e)
|
||||||
|
|
||||||
|
def pop_all(self):
|
||||||
|
"""Drain everything queued right now, oldest first."""
|
||||||
|
with self._lock:
|
||||||
|
out = list(self._chunks)
|
||||||
|
self._chunks.clear()
|
||||||
|
return out
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
try:
|
||||||
|
self.audio.unsubscribe(self._svc_name)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def _send(conn, msg_type, payload):
|
||||||
|
conn.sendall(struct.pack(">BI", msg_type, len(payload)))
|
||||||
|
conn.sendall(payload)
|
||||||
|
|
||||||
|
|
||||||
|
def serve_video(grabber, host, port):
|
||||||
|
"""Video gets its own connection/thread - a slow sendall() here
|
||||||
|
(a big JPEG frame over a bad link) must never be able to delay
|
||||||
|
audio, which used to share this same socket and paid for it in
|
||||||
|
latency."""
|
||||||
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||||
|
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||||
|
sock.bind((host, port))
|
||||||
|
sock.listen(1)
|
||||||
|
print("video server listening on :%d" % port)
|
||||||
|
while True:
|
||||||
|
conn, addr = sock.accept()
|
||||||
|
conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||||
|
conn.settimeout(5.0)
|
||||||
|
print("video client connected: %s" % (addr,))
|
||||||
|
last_jpeg = None
|
||||||
|
last_send = 0.0
|
||||||
|
try:
|
||||||
|
_send(conn, TYPE_HELLO, json.dumps({"stream": "video"}).encode("utf-8"))
|
||||||
|
while True:
|
||||||
|
jpeg = grabber.latest()
|
||||||
|
_, min_interval = grabber.quality_and_interval()
|
||||||
|
now = time.time()
|
||||||
|
if jpeg is not None and jpeg is not last_jpeg and now - last_send >= min_interval:
|
||||||
|
t0 = time.time()
|
||||||
|
_send(conn, TYPE_VIDEO, jpeg)
|
||||||
|
grabber.report_send_time(time.time() - t0)
|
||||||
|
last_jpeg = jpeg
|
||||||
|
last_send = now
|
||||||
|
else:
|
||||||
|
time.sleep(0.01)
|
||||||
|
except Exception as e:
|
||||||
|
print("video client disconnected (%s)" % e)
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
conn.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def serve_audio(mic, host, port):
|
||||||
|
"""Audio's own connection/thread. Checked eagerly (short sleep,
|
||||||
|
not paced like video) since gaps are far more noticeable than a
|
||||||
|
video frame being a beat stale."""
|
||||||
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||||
|
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||||
|
sock.bind((host, port))
|
||||||
|
sock.listen(1)
|
||||||
|
print("audio server listening on :%d" % port)
|
||||||
|
while True:
|
||||||
|
conn, addr = sock.accept()
|
||||||
|
conn.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
|
||||||
|
conn.settimeout(5.0)
|
||||||
|
print("audio client connected: %s" % (addr,))
|
||||||
|
try:
|
||||||
|
_send(conn, TYPE_HELLO,
|
||||||
|
json.dumps({"rate": AUDIO_RATE, "channels": AUDIO_CHANNELS}).encode("utf-8"))
|
||||||
|
while True:
|
||||||
|
chunks = mic.pop_all() if mic is not None else []
|
||||||
|
if chunks:
|
||||||
|
for chunk in chunks:
|
||||||
|
_send(conn, TYPE_AUDIO, chunk)
|
||||||
|
else:
|
||||||
|
time.sleep(0.005)
|
||||||
|
except Exception as e:
|
||||||
|
print("audio client disconnected (%s)" % e)
|
||||||
|
finally:
|
||||||
|
try:
|
||||||
|
conn.close()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
session = qi.Session()
|
||||||
|
session.connect("tcp://127.0.0.1:%d" % NAOQI_PORT)
|
||||||
|
print("connected to local NAOqi on port %d" % NAOQI_PORT)
|
||||||
|
|
||||||
|
grabber = FrameGrabber(session)
|
||||||
|
mic = None
|
||||||
|
try:
|
||||||
|
mic = MicGrabber(session)
|
||||||
|
print("mic capture ready (mu-law, %dHz)" % AUDIO_RATE)
|
||||||
|
except Exception as e:
|
||||||
|
print("mic capture unavailable (%s) - video only" % e)
|
||||||
|
|
||||||
|
video_thread = threading.Thread(target=serve_video, args=(grabber, "0.0.0.0", VIDEO_PORT))
|
||||||
|
video_thread.daemon = True
|
||||||
|
video_thread.start()
|
||||||
|
|
||||||
|
try:
|
||||||
|
serve_audio(mic, "0.0.0.0", AUDIO_PORT)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
grabber.close()
|
||||||
|
if mic:
|
||||||
|
mic.close()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
-101
@@ -1,101 +0,0 @@
|
|||||||
#!/usr/bin/env python
|
|
||||||
# -*- encoding: UTF-8 -*-
|
|
||||||
"""
|
|
||||||
Standalone NAO audio diagnostic - deliberately bypasses naowalk.py entirely.
|
|
||||||
|
|
||||||
Run this FIRST when troubleshooting "I can't hear the robot's mic". It tests
|
|
||||||
each link in the chain independently, so a failure tells you exactly where
|
|
||||||
the problem is instead of "somewhere in 300 lines of PyAudio/threading code":
|
|
||||||
|
|
||||||
TEST 1 - Front mic hardware energy.
|
|
||||||
Uses ALAudioDevice's built-in energy meter. No PyAudio, no
|
|
||||||
network audio streaming, no laptop audio stack involved at all.
|
|
||||||
This is the lowest-level check of "does the mic itself work".
|
|
||||||
|
|
||||||
TEST 2 - Local on-robot recording.
|
|
||||||
Records straight to a .wav file on the robot's own disk, then
|
|
||||||
you pull it off and play it normally. This bypasses the live
|
|
||||||
network stream, the SoundReceiver/queue/threading code, and
|
|
||||||
your laptop's PyAudio output path - all at once.
|
|
||||||
|
|
||||||
TEST 3 - Robot's own speaker.
|
|
||||||
Plays a tone through the ROBOT's speaker (not your laptop), to
|
|
||||||
confirm robot-side audio output is fine, independent of any of
|
|
||||||
the mic-listening code.
|
|
||||||
|
|
||||||
If TEST 1 shows no energy change when you make noise: mic hardware/mute/
|
|
||||||
environment issue, upstream of any code in this project.
|
|
||||||
If TEST 1 works but TEST 2's file is silent: something is wrong with the
|
|
||||||
recording/robot-audio-stack itself, still nothing to do with naowalk.py.
|
|
||||||
If TEST 2's file has clear audio: the mic and robot are fine, and the bug
|
|
||||||
is specifically in naowalk.py's live-streaming path (network delivery,
|
|
||||||
PyAudio device selection, etc.) - go back to the diagnostics already
|
|
||||||
built into naowalk.py (device list / test beep / peak level) to isolate it
|
|
||||||
further.
|
|
||||||
|
|
||||||
Usage:
|
|
||||||
python3 nao_audio_diagnostic.py --ip spike.local --port 9559
|
|
||||||
"""
|
|
||||||
|
|
||||||
import qi
|
|
||||||
import argparse
|
|
||||||
import time
|
|
||||||
|
|
||||||
parser = argparse.ArgumentParser()
|
|
||||||
parser.add_argument("--ip", type=str, default="127.0.0.1")
|
|
||||||
parser.add_argument("--port", type=int, default=9559)
|
|
||||||
args = parser.parse_args()
|
|
||||||
|
|
||||||
session = qi.Session()
|
|
||||||
session.connect(f"tcp://{args.ip}:{args.port}")
|
|
||||||
print(f"✅ Connected to {args.ip}")
|
|
||||||
audio = session.service("ALAudioDevice")
|
|
||||||
|
|
||||||
# --- TEST 1: mic hardware energy, all four channels ------------------------
|
|
||||||
print("\n=== TEST 1: Mic hardware energy (all 4 mics) ===")
|
|
||||||
print("Make noise near the robot's head (clap, talk right up close to it) for 10s...")
|
|
||||||
audio.enableEnergyComputation()
|
|
||||||
front_readings, rear_readings, left_readings, right_readings = [], [], [], []
|
|
||||||
for i in range(10):
|
|
||||||
f, r, l, rt = (audio.getFrontMicEnergy(), audio.getRearMicEnergy(),
|
|
||||||
audio.getLeftMicEnergy(), audio.getRightMicEnergy())
|
|
||||||
front_readings.append(f); rear_readings.append(r); left_readings.append(l); right_readings.append(rt)
|
|
||||||
print(f" t={i}s front={f:.2f} rear={r:.2f} left={l:.2f} right={rt:.2f}")
|
|
||||||
time.sleep(1)
|
|
||||||
audio.disableEnergyComputation()
|
|
||||||
|
|
||||||
results = {"front": front_readings, "rear": rear_readings, "left": left_readings, "right": right_readings}
|
|
||||||
flat = [name for name, vals in results.items() if max(vals) - min(vals) < 2]
|
|
||||||
if len(flat) == 4:
|
|
||||||
print("⚠️ ALL FOUR mics are flat. This points to something systemic - e.g. audio "
|
|
||||||
"inputs closed (closeAudioInputs() called somewhere and never reopened), "
|
|
||||||
"or a low-level hardware/driver fault - rather than one damaged mic.")
|
|
||||||
elif flat:
|
|
||||||
print(f"⚠️ Flat mic(s): {', '.join(flat)}. The others responded normally, which "
|
|
||||||
f"points to a fault specific to {', '.join(flat)} rather than the audio "
|
|
||||||
f"pipeline as a whole (that pipeline is clearly working for the others).")
|
|
||||||
else:
|
|
||||||
print("✅ All four mics responded to noise - hardware capture is fine across the board.")
|
|
||||||
|
|
||||||
# --- TEST 2: local on-robot recording ---------------------------------------
|
|
||||||
print("\n=== TEST 2: Local on-robot recording ===")
|
|
||||||
remote_path = "/home/nao/mic_test.wav"
|
|
||||||
print(f"Recording 5s directly to {remote_path} on the robot itself "
|
|
||||||
f"(this bypasses the network stream and your laptop's audio stack entirely)...")
|
|
||||||
audio.startMicrophonesRecording(remote_path)
|
|
||||||
time.sleep(5)
|
|
||||||
audio.stopMicrophonesRecording()
|
|
||||||
print(f"Done. Pull it off and play it with:")
|
|
||||||
print(f" scp nao@{args.ip}:{remote_path} .")
|
|
||||||
print(f" (then open mic_test.wav in any media player - it's 4-channel, so check all channels)")
|
|
||||||
print("If that file has clearly audible sound, the mic and robot are fine and the")
|
|
||||||
print("bug is specifically in naowalk.py's live-streaming path.")
|
|
||||||
|
|
||||||
# --- TEST 3: robot's own speaker, via TTS (version-agnostic) ---------------
|
|
||||||
print("\n=== TEST 3: Robot's own speaker ===")
|
|
||||||
input("Press Enter to have the robot say a test phrase out loud...")
|
|
||||||
session.service("ALTextToSpeech").say("Testing 1 2 3, can you hear me.")
|
|
||||||
print("If you heard the robot speak just now, its speaker output is fine "
|
|
||||||
"(this is unrelated to hearing its mic on your laptop, but rules out a robot-wide audio fault).")
|
|
||||||
|
|
||||||
print("\nDone. Report back which test(s) failed and we'll know exactly where to look.")
|
|
||||||
+6
-3
@@ -9,15 +9,17 @@ import time
|
|||||||
import threading
|
import threading
|
||||||
|
|
||||||
class NaoMusicPlayer:
|
class NaoMusicPlayer:
|
||||||
def __init__(self, session, nao_ip):
|
def __init__(self, session, nao_ip, ssh_port=22):
|
||||||
self.session = session
|
self.session = session
|
||||||
self.player = session.service("ALAudioPlayer")
|
self.player = session.service("ALAudioPlayer")
|
||||||
self.nao_ip = nao_ip
|
self.nao_ip = nao_ip
|
||||||
|
self.ssh_port = ssh_port
|
||||||
self.music_dir = "/home/nao/music"
|
self.music_dir = "/home/nao/music"
|
||||||
self.local_path = "/tmp/song.mp3"
|
self.local_path = "/tmp/song.mp3"
|
||||||
self.remote_path = f"{self.music_dir}/song.mp3"
|
self.remote_path = f"{self.music_dir}/song.mp3"
|
||||||
try:
|
try:
|
||||||
subprocess.run(["sshpass", "-p", "nao", "ssh", f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"],
|
subprocess.run(["sshpass", "-p", "nao", "ssh", "-p", str(self.ssh_port),
|
||||||
|
f"nao@{nao_ip}", f"mkdir -p {self.music_dir}"],
|
||||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
|
||||||
except: pass
|
except: pass
|
||||||
self.current_file = None
|
self.current_file = None
|
||||||
@@ -110,7 +112,8 @@ class NaoMusicPlayer:
|
|||||||
t = threading.Thread(target=ticker, daemon=True)
|
t = threading.Thread(target=ticker, daemon=True)
|
||||||
t.start()
|
t.start()
|
||||||
scp_result = subprocess.run(
|
scp_result = subprocess.run(
|
||||||
["sshpass", "-p", "nao", "scp", self.local_path, f"nao@{self.nao_ip}:{self.remote_path}"],
|
["sshpass", "-p", "nao", "scp", "-P", str(self.ssh_port),
|
||||||
|
self.local_path, f"nao@{self.nao_ip}:{self.remote_path}"],
|
||||||
capture_output=True, text=True, timeout=60
|
capture_output=True, text=True, timeout=60
|
||||||
)
|
)
|
||||||
stop_ticker.set()
|
stop_ticker.set()
|
||||||
|
|||||||
+390
-39
@@ -13,7 +13,7 @@ import pygame
|
|||||||
import subprocess
|
import subprocess
|
||||||
import os
|
import os
|
||||||
import json
|
import json
|
||||||
import re
|
import struct
|
||||||
import socket
|
import socket
|
||||||
|
|
||||||
# Import the new music module
|
# Import the new music module
|
||||||
@@ -33,6 +33,38 @@ try:
|
|||||||
except ImportError:
|
except ImportError:
|
||||||
HAS_SSH = False
|
HAS_SSH = False
|
||||||
|
|
||||||
|
# ==========================================
|
||||||
|
# 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.
|
||||||
|
# ==========================================
|
||||||
|
TYPE_HELLO = 0x00
|
||||||
|
TYPE_VIDEO = 0x01
|
||||||
|
TYPE_AUDIO = 0x02
|
||||||
|
|
||||||
|
|
||||||
|
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 _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
|
||||||
|
|
||||||
|
|
||||||
# ==========================================
|
# ==========================================
|
||||||
# NAO MIC STREAM
|
# NAO MIC STREAM
|
||||||
# ==========================================
|
# ==========================================
|
||||||
@@ -82,14 +114,22 @@ class SoundReceiver:
|
|||||||
except: pass
|
except: pass
|
||||||
|
|
||||||
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
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
|
if not HAS_AUDIO: return
|
||||||
self.chunks_received += 1
|
self.chunks_received += 1
|
||||||
try:
|
try:
|
||||||
self.queue.put_nowait(bytes(buffer))
|
self.queue.put_nowait(raw_pcm_bytes)
|
||||||
except queue.Full:
|
except queue.Full:
|
||||||
try:
|
try:
|
||||||
self.queue.get_nowait()
|
self.queue.get_nowait()
|
||||||
self.queue.put_nowait(bytes(buffer))
|
self.queue.put_nowait(raw_pcm_bytes)
|
||||||
except: pass
|
except: pass
|
||||||
|
|
||||||
def _playback_loop(self):
|
def _playback_loop(self):
|
||||||
@@ -163,11 +203,12 @@ class LiveMicRelay:
|
|||||||
nc so it's ready for the next press. close() (call once at shutdown)
|
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.
|
tears down the persistent SSH connection and kills the remote loop.
|
||||||
"""
|
"""
|
||||||
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao",
|
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):
|
port=5555, rate=16000, input_device_index=None, pyaudio_instance=None):
|
||||||
self.nao_ip = nao_ip
|
self.nao_ip = nao_ip
|
||||||
self.ssh_user = ssh_user
|
self.ssh_user = ssh_user
|
||||||
self.ssh_password = ssh_password
|
self.ssh_password = ssh_password
|
||||||
|
self.ssh_port = ssh_port
|
||||||
self.port = port
|
self.port = port
|
||||||
self.rate = rate
|
self.rate = rate
|
||||||
self.input_device_index = input_device_index
|
self.input_device_index = input_device_index
|
||||||
@@ -199,7 +240,7 @@ class LiveMicRelay:
|
|||||||
try:
|
try:
|
||||||
self._ssh_client = paramiko.SSHClient()
|
self._ssh_client = paramiko.SSHClient()
|
||||||
self._ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
self._ssh_client.set_missing_host_key_policy(paramiko.AutoAddPolicy())
|
||||||
self._ssh_client.connect(self.nao_ip, username=self.ssh_user,
|
self._ssh_client.connect(self.nao_ip, port=self.ssh_port, username=self.ssh_user,
|
||||||
password=self.ssh_password, timeout=5)
|
password=self.ssh_password, timeout=5)
|
||||||
self._ssh_channel = self._ssh_client.get_transport().open_session()
|
self._ssh_channel = self._ssh_client.get_transport().open_session()
|
||||||
# A pty ties the remote process group to this channel, so
|
# A pty ties the remote process group to this channel, so
|
||||||
@@ -306,12 +347,140 @@ class LiveMicRelay:
|
|||||||
self._cleanup()
|
self._cleanup()
|
||||||
print("🎙️ Talk relay: stopped")
|
print("🎙️ Talk relay: stopped")
|
||||||
|
|
||||||
|
# ==========================================
|
||||||
|
# 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
|
||||||
|
|
||||||
|
|
||||||
# ==========================================
|
# ==========================================
|
||||||
# MAIN TELEOP
|
# MAIN TELEOP
|
||||||
# ==========================================
|
# ==========================================
|
||||||
class NaoTeleop:
|
class NaoTeleop:
|
||||||
def __init__(self, session, nao_ip, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0,
|
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", relay_port=5555, relay_rate=16000):
|
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):
|
||||||
self.session = session
|
self.session = session
|
||||||
self.nao_ip = nao_ip
|
self.nao_ip = nao_ip
|
||||||
self.motion = session.service("ALMotion")
|
self.motion = session.service("ALMotion")
|
||||||
@@ -322,7 +491,7 @@ class NaoTeleop:
|
|||||||
except:
|
except:
|
||||||
self.battery = None
|
self.battery = None
|
||||||
|
|
||||||
self.music = NaoMusicPlayer(session, nao_ip)
|
self.music = NaoMusicPlayer(session, nao_ip, ssh_port=nao_ssh_port)
|
||||||
self.music_results = []
|
self.music_results = []
|
||||||
self.selected_result = 0
|
self.selected_result = 0
|
||||||
|
|
||||||
@@ -338,6 +507,7 @@ class NaoTeleop:
|
|||||||
self._pyaudio = pyaudio.PyAudio() if HAS_AUDIO else None
|
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,
|
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)
|
port=relay_port, rate=relay_rate, pyaudio_instance=self._pyaudio)
|
||||||
self.ptt_active = False
|
self.ptt_active = False
|
||||||
|
|
||||||
@@ -356,9 +526,37 @@ class NaoTeleop:
|
|||||||
self.music_search_pending = False
|
self.music_search_pending = False
|
||||||
|
|
||||||
self.video_scale = video_scale
|
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_width = int(320 * video_scale)
|
||||||
self.display_height = int(240 * 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
|
||||||
|
|
||||||
|
# If we're using the on-robot media relay, deploy+launch
|
||||||
|
# nao_video_server.py over SSH now (before _init_video_maxfps
|
||||||
|
# tries to connect to it) instead of requiring it to already be
|
||||||
|
# running manually.
|
||||||
|
self.video_server_launcher = None
|
||||||
|
if self.media_relay and 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_audio_device()
|
||||||
self._init_video_maxfps()
|
self._init_video_maxfps()
|
||||||
if HAS_AUDIO:
|
if HAS_AUDIO:
|
||||||
@@ -388,41 +586,133 @@ class NaoTeleop:
|
|||||||
self.audio_device = None
|
self.audio_device = None
|
||||||
|
|
||||||
def _init_video_maxfps(self):
|
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:
|
try:
|
||||||
self.video = self.session.service("ALVideoDevice")
|
self.video = self.session.service("ALVideoDevice")
|
||||||
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, 25)
|
self.video_client = self.video.subscribeCamera("TeleopCam", 0, 0, 11, self.video_fps)
|
||||||
print("✅ Camera ready")
|
print("✅ Camera ready")
|
||||||
|
threading.Thread(target=self._video_loop, daemon=True).start()
|
||||||
except:
|
except:
|
||||||
print("❌ Camera not available")
|
print("❌ Camera not available")
|
||||||
self.video_client = None
|
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_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)."""
|
||||||
|
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)."""
|
||||||
|
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)
|
||||||
|
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:
|
||||||
|
if sock:
|
||||||
|
try: sock.close()
|
||||||
|
except: pass
|
||||||
|
if not self._video_stop_event.is_set():
|
||||||
|
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):
|
def _init_mic_stream(self):
|
||||||
if not self.audio_device: return
|
if not self.audio_device: return
|
||||||
try:
|
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,
|
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
||||||
pyaudio_instance=self._pyaudio)
|
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)
|
self.session.registerService(self.audio_service_name, self.sound_receiver)
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
||||||
self.audio_device.subscribe(self.audio_service_name)
|
self.audio_device.subscribe(self.audio_service_name)
|
||||||
print(f"✅ Mic stream ready (gain={self.mic_gain}x)")
|
self._qi_audio_subscribed = True
|
||||||
|
print(f"✅ Mic stream ready (gain={self.mic_gain}x, uncompressed direct - use --media-relay on a tunneled link)")
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f"❌ Mic init failed: {e}")
|
print(f"❌ Mic init failed: {e}")
|
||||||
|
|
||||||
def get_image(self):
|
|
||||||
if not hasattr(self, 'video') or not self.video_client:
|
|
||||||
return None
|
|
||||||
try:
|
|
||||||
image = self.video.getImageRemote(self.video_client)
|
|
||||||
if image and len(image) >= 7:
|
|
||||||
w, h = image[0], image[1]
|
|
||||||
arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3))
|
|
||||||
frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
|
||||||
return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
|
||||||
except:
|
|
||||||
pass
|
|
||||||
return None
|
|
||||||
|
|
||||||
def wave_gesture(self):
|
def wave_gesture(self):
|
||||||
if self.typing_mode: return
|
if self.typing_mode: return
|
||||||
print("🤚 Waving hello...")
|
print("🤚 Waving hello...")
|
||||||
@@ -552,6 +842,32 @@ class NaoTeleop:
|
|||||||
self.update_title()
|
self.update_title()
|
||||||
self.last_batt_check = now
|
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):
|
def run(self):
|
||||||
print("🤖 Enabling stiffness (no wakeUp stand-up)...")
|
print("🤖 Enabling stiffness (no wakeUp stand-up)...")
|
||||||
self.motion.setStiffnesses("Body", 1.0)
|
self.motion.setStiffnesses("Body", 1.0)
|
||||||
@@ -570,6 +886,9 @@ class NaoTeleop:
|
|||||||
if getattr(self, "mic_relay", None):
|
if getattr(self, "mic_relay", None):
|
||||||
threading.Thread(target=self.mic_relay.connect, daemon=True).start()
|
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")
|
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:
|
try:
|
||||||
@@ -698,19 +1017,11 @@ class NaoTeleop:
|
|||||||
if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = self.walk_speed * 0.8
|
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
|
if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -self.walk_speed * 0.8
|
||||||
|
|
||||||
try:
|
# Just record intent here - the movement thread
|
||||||
if abs(x) > 0.05 or abs(theta) > 0.05:
|
# (see _movement_loop) is what actually calls
|
||||||
stable_config = [
|
# moveToward/stopMove, so a stalled network RPC
|
||||||
["StepHeight", 0.02],
|
# can't stop us reading the next key state.
|
||||||
["MaxStepFrequency", 0.5],
|
self._desired_velocity = (x, y, theta)
|
||||||
["TorsoWy", 0.08],
|
|
||||||
["MaxStepX", 0.03]
|
|
||||||
]
|
|
||||||
self.motion.moveToward(x, y, theta, stable_config)
|
|
||||||
else:
|
|
||||||
self.motion.stopMove()
|
|
||||||
except Exception as e:
|
|
||||||
print(f"Move error: {e}")
|
|
||||||
self.handle_head_movement()
|
self.handle_head_movement()
|
||||||
|
|
||||||
frame = self.get_image()
|
frame = self.get_image()
|
||||||
@@ -719,6 +1030,9 @@ class NaoTeleop:
|
|||||||
rgb = cv2.cvtColor(up, cv2.COLOR_BGR2RGB)
|
rgb = cv2.cvtColor(up, cv2.COLOR_BGR2RGB)
|
||||||
surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1))
|
surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1))
|
||||||
self.screen.blit(surf, (0, 0))
|
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:
|
else:
|
||||||
self.screen.fill((10, 10, 30))
|
self.screen.fill((10, 10, 30))
|
||||||
|
|
||||||
@@ -812,16 +1126,22 @@ class NaoTeleop:
|
|||||||
print(f"\u26a0\ufe0f Frame error (continuing): {e}")
|
print(f"\u26a0\ufe0f Frame error (continuing): {e}")
|
||||||
finally:
|
finally:
|
||||||
print("🛑 Shutting down...")
|
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("stopMove", self.motion.stopMove)
|
||||||
self._safe("music stop", self.music.stop)
|
self._safe("music stop", self.music.stop)
|
||||||
if getattr(self, "mic_relay", None):
|
if getattr(self, "mic_relay", None):
|
||||||
self._safe("mic relay stop", self.mic_relay.stop)
|
self._safe("mic relay stop", self.mic_relay.stop)
|
||||||
self._safe("mic relay close", self.mic_relay.close)
|
self._safe("mic relay close", self.mic_relay.close)
|
||||||
if self.audio_device:
|
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))
|
self._safe("audio unsubscribe", lambda: self.audio_device.unsubscribe(self.audio_service_name))
|
||||||
if getattr(self, "sound_receiver", None):
|
if getattr(self, "sound_receiver", None):
|
||||||
self._safe("sound receiver close", self.sound_receiver.close)
|
self._safe("sound receiver close", self.sound_receiver.close)
|
||||||
if hasattr(self, 'video') and self.video_client:
|
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))
|
self._safe("video unsubscribe", lambda: self.video.unsubscribe(self.video_client))
|
||||||
if getattr(self, "_pyaudio", None):
|
if getattr(self, "_pyaudio", None):
|
||||||
self._safe("pyaudio terminate", self._pyaudio.terminate)
|
self._safe("pyaudio terminate", self._pyaudio.terminate)
|
||||||
@@ -839,14 +1159,40 @@ if __name__ == "__main__":
|
|||||||
parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left")
|
parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left")
|
||||||
parser.add_argument("--mic-gain", type=float, default=8.0)
|
parser.add_argument("--mic-gain", type=float, default=8.0)
|
||||||
parser.add_argument("--video-scale", type=float, default=2.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 "
|
||||||
|
"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 - 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).")
|
||||||
parser.add_argument("--nao-ssh-user", type=str, default="nao",
|
parser.add_argument("--nao-ssh-user", type=str, default="nao",
|
||||||
help="SSH login for the live mic relay (push-to-talk)")
|
help="SSH login for the live mic relay (push-to-talk)")
|
||||||
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
||||||
help="SSH password for the live mic relay (push-to-talk)")
|
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,
|
parser.add_argument("--relay-port", type=int, default=5555,
|
||||||
help="TCP port used to stream mic audio to the robot's nc listener")
|
help="TCP port used to stream mic audio to the robot's nc listener")
|
||||||
parser.add_argument("--relay-rate", type=int, default=16000,
|
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")
|
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)")
|
||||||
args = parser.parse_args()
|
args = parser.parse_args()
|
||||||
|
|
||||||
session = qi.Session()
|
session = qi.Session()
|
||||||
@@ -862,7 +1208,12 @@ if __name__ == "__main__":
|
|||||||
mic_channel=MIC_CHANNELS[args.mic_channel],
|
mic_channel=MIC_CHANNELS[args.mic_channel],
|
||||||
mic_gain=args.mic_gain,
|
mic_gain=args.mic_gain,
|
||||||
video_scale=args.video_scale,
|
video_scale=args.video_scale,
|
||||||
|
video_fps=args.video_fps,
|
||||||
|
media_relay=args.media_relay,
|
||||||
nao_ssh_user=args.nao_ssh_user,
|
nao_ssh_user=args.nao_ssh_user,
|
||||||
nao_ssh_password=args.nao_ssh_password,
|
nao_ssh_password=args.nao_ssh_password,
|
||||||
|
nao_ssh_port=args.nao_ssh_port,
|
||||||
relay_port=args.relay_port,
|
relay_port=args.relay_port,
|
||||||
relay_rate=args.relay_rate).run()
|
relay_rate=args.relay_rate,
|
||||||
|
deploy_video_server=not args.no_deploy_video_server,
|
||||||
|
video_server_path=args.video_server_path).run()
|
||||||
|
|||||||
@@ -1,3 +1,5 @@
|
|||||||
#!/bin/bash
|
#!/bin/bash
|
||||||
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
pactl set-source-volume alsa_input.pci-0000_00_1b.0.analog-stereo 32%
|
||||||
python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
#python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0
|
||||||
|
#python3.11 naowalk.py --ip 127.0.0.1 --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 2222 --media-relay 127.0.0.1:8000
|
||||||
|
python3.11 naowalk.py --ip spike.local --port 9561 --mic-gain 9 --video-scale 3.0 --nao-ssh-port 22 --media-relay spike.local:8000
|
||||||
|
|||||||
Executable
+1
@@ -0,0 +1 @@
|
|||||||
|
ssh -N -L 9561:192.168.0.100:9561 -L 2222:192.168.0.100:22 -L 8000:192.168.0.100:8000 root@10.196.124.46
|
||||||
Reference in New Issue
Block a user