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
|
||||
|
||||
class NaoMusicPlayer:
|
||||
def __init__(self, session, nao_ip):
|
||||
def __init__(self, session, nao_ip, ssh_port=22):
|
||||
self.session = session
|
||||
self.player = session.service("ALAudioPlayer")
|
||||
self.nao_ip = nao_ip
|
||||
self.ssh_port = ssh_port
|
||||
self.music_dir = "/home/nao/music"
|
||||
self.local_path = "/tmp/song.mp3"
|
||||
self.remote_path = f"{self.music_dir}/song.mp3"
|
||||
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)
|
||||
except: pass
|
||||
self.current_file = None
|
||||
@@ -110,7 +112,8 @@ class NaoMusicPlayer:
|
||||
t = threading.Thread(target=ticker, daemon=True)
|
||||
t.start()
|
||||
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
|
||||
)
|
||||
stop_ticker.set()
|
||||
|
||||
+395
-44
@@ -13,7 +13,7 @@ import pygame
|
||||
import subprocess
|
||||
import os
|
||||
import json
|
||||
import re
|
||||
import struct
|
||||
import socket
|
||||
|
||||
# Import the new music module
|
||||
@@ -33,6 +33,38 @@ try:
|
||||
except ImportError:
|
||||
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
|
||||
# ==========================================
|
||||
@@ -82,14 +114,22 @@ class SoundReceiver:
|
||||
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(bytes(buffer))
|
||||
self.queue.put_nowait(raw_pcm_bytes)
|
||||
except queue.Full:
|
||||
try:
|
||||
self.queue.get_nowait()
|
||||
self.queue.put_nowait(bytes(buffer))
|
||||
self.queue.put_nowait(raw_pcm_bytes)
|
||||
except: pass
|
||||
|
||||
def _playback_loop(self):
|
||||
@@ -163,11 +203,12 @@ class LiveMicRelay:
|
||||
nc so it's ready for the next press. close() (call once at shutdown)
|
||||
tears down the persistent SSH connection and kills the remote loop.
|
||||
"""
|
||||
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao",
|
||||
def __init__(self, nao_ip, ssh_user="nao", ssh_password="nao", ssh_port=22,
|
||||
port=5555, rate=16000, input_device_index=None, pyaudio_instance=None):
|
||||
self.nao_ip = nao_ip
|
||||
self.ssh_user = ssh_user
|
||||
self.ssh_password = ssh_password
|
||||
self.ssh_port = ssh_port
|
||||
self.port = port
|
||||
self.rate = rate
|
||||
self.input_device_index = input_device_index
|
||||
@@ -199,7 +240,7 @@ class LiveMicRelay:
|
||||
try:
|
||||
self._ssh_client = paramiko.SSHClient()
|
||||
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)
|
||||
self._ssh_channel = self._ssh_client.get_transport().open_session()
|
||||
# A pty ties the remote process group to this channel, so
|
||||
@@ -306,12 +347,140 @@ class LiveMicRelay:
|
||||
self._cleanup()
|
||||
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
|
||||
# ==========================================
|
||||
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", 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.nao_ip = nao_ip
|
||||
self.motion = session.service("ALMotion")
|
||||
@@ -322,7 +491,7 @@ class NaoTeleop:
|
||||
except:
|
||||
self.battery = None
|
||||
|
||||
self.music = NaoMusicPlayer(session, nao_ip)
|
||||
self.music = NaoMusicPlayer(session, nao_ip, ssh_port=nao_ssh_port)
|
||||
self.music_results = []
|
||||
self.selected_result = 0
|
||||
|
||||
@@ -338,6 +507,7 @@ class NaoTeleop:
|
||||
self._pyaudio = pyaudio.PyAudio() if HAS_AUDIO else None
|
||||
|
||||
self.mic_relay = LiveMicRelay(nao_ip, ssh_user=nao_ssh_user, ssh_password=nao_ssh_password,
|
||||
ssh_port=nao_ssh_port,
|
||||
port=relay_port, rate=relay_rate, pyaudio_instance=self._pyaudio)
|
||||
self.ptt_active = False
|
||||
|
||||
@@ -356,9 +526,37 @@ 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
|
||||
# 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_video_maxfps()
|
||||
if HAS_AUDIO:
|
||||
@@ -388,40 +586,132 @@ class NaoTeleop:
|
||||
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, 25)
|
||||
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 _init_mic_stream(self):
|
||||
if not self.audio_device: return
|
||||
try:
|
||||
self.sound_receiver = SoundReceiver(self.audio_device_index, self.mic_gain,
|
||||
pyaudio_instance=self._pyaudio)
|
||||
self.session.registerService(self.audio_service_name, self.sound_receiver)
|
||||
time.sleep(0.5)
|
||||
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
|
||||
self.audio_device.subscribe(self.audio_service_name)
|
||||
print(f"✅ Mic stream ready (gain={self.mic_gain}x)")
|
||||
except Exception as e:
|
||||
print(f"❌ Mic init failed: {e}")
|
||||
|
||||
def get_image(self):
|
||||
if not hasattr(self, 'video') or not self.video_client:
|
||||
return 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)
|
||||
return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
||||
except:
|
||||
pass
|
||||
return None
|
||||
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):
|
||||
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)")
|
||||
except Exception as e:
|
||||
print(f"❌ Mic init failed: {e}")
|
||||
|
||||
def wave_gesture(self):
|
||||
if self.typing_mode: return
|
||||
@@ -552,6 +842,32 @@ class NaoTeleop:
|
||||
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)
|
||||
@@ -570,6 +886,9 @@ class NaoTeleop:
|
||||
if getattr(self, "mic_relay", None):
|
||||
threading.Thread(target=self.mic_relay.connect, daemon=True).start()
|
||||
|
||||
self._movement_thread = threading.Thread(target=self._movement_loop, daemon=True)
|
||||
self._movement_thread.start()
|
||||
|
||||
print("\n🎮 Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | 2=Cena | 3=6-7 | M=Music search | X=Stop | -/==Volume | T=TTS | V=Push-to-talk (hold) | ESC=Quit")
|
||||
|
||||
try:
|
||||
@@ -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_d] or keys[pygame.K_RIGHT]: theta = -self.walk_speed * 0.8
|
||||
|
||||
try:
|
||||
if abs(x) > 0.05 or abs(theta) > 0.05:
|
||||
stable_config = [
|
||||
["StepHeight", 0.02],
|
||||
["MaxStepFrequency", 0.5],
|
||||
["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}")
|
||||
# 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()
|
||||
@@ -719,6 +1030,9 @@ class NaoTeleop:
|
||||
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))
|
||||
|
||||
@@ -812,16 +1126,22 @@ class NaoTeleop:
|
||||
print(f"\u26a0\ufe0f Frame error (continuing): {e}")
|
||||
finally:
|
||||
print("🛑 Shutting down...")
|
||||
self._movement_stop_event.set()
|
||||
if self._movement_thread:
|
||||
self._movement_thread.join(timeout=1.0)
|
||||
self._video_stop_event.set()
|
||||
self._safe("stopMove", self.motion.stopMove)
|
||||
self._safe("music stop", self.music.stop)
|
||||
if getattr(self, "mic_relay", None):
|
||||
self._safe("mic relay stop", self.mic_relay.stop)
|
||||
self._safe("mic relay close", self.mic_relay.close)
|
||||
if self.audio_device:
|
||||
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_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))
|
||||
if getattr(self, "_pyaudio", None):
|
||||
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-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 "
|
||||
"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",
|
||||
help="SSH login for the live mic relay (push-to-talk)")
|
||||
parser.add_argument("--nao-ssh-password", type=str, default="nao",
|
||||
help="SSH password for the live mic relay (push-to-talk)")
|
||||
parser.add_argument("--nao-ssh-port", type=int, default=22,
|
||||
help="SSH port for the robot (music upload + push-to-talk). "
|
||||
"Set this to your local forwarded port, e.g. 2222, when tunneling.")
|
||||
parser.add_argument("--relay-port", type=int, default=5555,
|
||||
help="TCP port used to stream mic audio to the robot's nc listener")
|
||||
parser.add_argument("--relay-rate", type=int, default=16000,
|
||||
help="Sample rate for the live mic relay - try 48000 if audio sounds sped up/slow")
|
||||
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()
|
||||
|
||||
session = qi.Session()
|
||||
@@ -862,7 +1208,12 @@ if __name__ == "__main__":
|
||||
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).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
|
||||
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