Compare commits

..

4 Commits

3 changed files with 1477 additions and 61 deletions
+1
View File
@@ -1 +1,2 @@
https://github.com/iikoshteruu/enhanced-grok-export https://github.com/iikoshteruu/enhanced-grok-export
https://github.com/revivalstack/ai-chat-exporter
+342 -51
View File
@@ -5,40 +5,240 @@ import qi
import argparse import argparse
import sys import sys
import time import time
import pygame import threading
import queue
import cv2 import cv2
import numpy as np import numpy as np
import pygame
# --- NEW: Audio Library ---
try:
import pyaudio
HAS_AUDIO = True
except ImportError:
HAS_AUDIO = False
print("⚠️ PyAudio not installed. Run 'pip install pyaudio' to hear the robot.")
# ==========================================
# AUDIO RECEIVER SERVICE (Runs in background)
# ==========================================
class SoundReceiver:
# How many chunks to buffer before we start playback. This absorbs Wi-Fi
# jitter so a late/slow packet doesn't starve ALSA the instant it lands.
PREBUFFER_CHUNKS = 3
# Silence written to the output stream whenever the queue runs dry, so we
# feed ALSA continuously instead of letting it hard-underrun (which is
# what produces clicking/silence).
SILENCE_CHUNK = b"\x00" * 4096
def __init__(self, output_device_index=None):
self.running = True
self.underrun_count = 0
self.chunks_received = 0
self._write_errors = 0
self._peak_since_print = 0
self._last_status_print = 0
if HAS_AUDIO:
self.p = pyaudio.PyAudio()
# Print every playback-capable device so you can tell whether
# PortAudio's "default" is actually your laptop speakers (on
# Linux it very often picks an HDMI or dummy device instead).
print("🔈 Available output devices:")
for i in range(self.p.get_device_count()):
info = self.p.get_device_info_by_index(i)
if info.get("maxOutputChannels", 0) > 0:
print(f" [{i}] {info['name']}")
try:
chosen = (self.p.get_device_info_by_index(output_device_index)
if output_device_index is not None
else self.p.get_default_output_device_info())
print(f"🔈 Using output device: [{chosen['index']}] {chosen['name']}"
f"{' (forced via --audio-device-index)' if output_device_index is not None else ' (default)'}")
except Exception as e:
print(f"⚠️ Could not resolve output device: {e}")
# NAO front mic is usually 16000Hz, 1 channel, 16-bit.
self.stream = self.p.open(format=pyaudio.paInt16,
channels=1,
rate=16000,
output=True,
output_device_index=output_device_index,
frames_per_buffer=2048)
self._play_test_tone()
# The queue decouples the NAOqi network thread (which calls
# processRemote) from the actual blocking audio write. Without
# this, a slow/blocking stream.write() call inside processRemote
# stalls NAOqi's callback thread, which delays the next packet,
# which starves the speaker further - a feedback loop that shows
# up as underruns and dropouts.
self.queue = queue.Queue(maxsize=40)
self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True)
self.playback_thread.start()
def _play_test_tone(self):
# Plays a short beep straight to the output device, independent of
# any data from the robot. If you don't hear this, the problem is
# your laptop's audio routing/volume, not the robot or the network -
# troubleshoot with alsamixer / your system's sound settings first.
try:
duration, freq, rate = 0.3, 440.0, 16000
t = np.linspace(0, duration, int(rate * duration), False)
tone = (np.sin(freq * t * 2 * np.pi) * 12000).astype(np.int16)
self.stream.write(tone.tobytes())
print("🔔 Played a test beep. If you didn't hear it, this is a laptop "
"audio output/device problem, not a robot/network problem.")
except Exception as e:
print(f"⚠️ Test tone failed to play: {e}")
# ALAudioDevice strictly requires this exact method signature to send data.
# Keep this method as fast as possible - it runs on NAOqi's network thread.
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
if not HAS_AUDIO:
return
self.chunks_received += 1
try:
self.queue.put_nowait(bytes(buffer))
except queue.Full:
# We're falling behind - drop the oldest chunk rather than
# blocking the NAOqi thread (blocking here is what causes
# cascading underruns).
try:
self.queue.get_nowait()
self.queue.put_nowait(bytes(buffer))
except queue.Empty:
pass
def _playback_loop(self):
# Wait for a small pile of chunks before we start playing, so the
# very first sounds you hear aren't immediately starved.
primed = []
while self.running and len(primed) < self.PREBUFFER_CHUNKS:
try:
primed.append(self.queue.get(timeout=1.0))
except queue.Empty:
break
for chunk in primed:
self._write_chunk(chunk)
while self.running:
try:
chunk = self.queue.get(timeout=0.2)
self._write_chunk(chunk)
except queue.Empty:
# Nothing arrived in time - feed silence instead of letting
# the ALSA buffer run completely dry.
self.underrun_count += 1
try:
self.stream.write(self.SILENCE_CHUNK)
except Exception:
pass
now = time.time()
if now - self._last_status_print > 10:
self._last_status_print = now
print(f"🎤 audio: {self.chunks_received} chunks received, "
f"{self.underrun_count} underruns, {self._write_errors} write errors, "
f"queue depth {self.queue.qsize()}, peak level {self._peak_since_print}/32767")
self._peak_since_print = 0
def _write_chunk(self, raw_bytes):
try:
self.stream.write(raw_bytes)
samples = np.frombuffer(raw_bytes, dtype=np.int16)
if samples.size:
peak = int(np.abs(samples).max())
if peak > self._peak_since_print:
self._peak_since_print = peak
except Exception as e:
self._write_errors += 1
if self._write_errors <= 3:
print(f"⚠️ audio write error: {e}")
def close(self):
self.running = False
if HAS_AUDIO:
try:
self.playback_thread.join(timeout=1.0)
except Exception:
pass
try:
self.stream.stop_stream()
self.stream.close()
self.p.terminate()
except Exception:
pass
# ==========================================
# MAIN TELEOP CLASS
# ==========================================
class NaoTeleop: class NaoTeleop:
def __init__(self, session): def __init__(self, session, audio_device_index=None):
self.session = session self.session = session
self.motion = session.service("ALMotion") self.motion = session.service("ALMotion")
self.posture = session.service("ALRobotPosture") self.posture = session.service("ALRobotPosture")
self.tts = session.service("ALTextToSpeech")
# New: Hook into the battery service
try: try:
self.battery = session.service("ALBattery") self.battery = session.service("ALBattery")
except: except:
self.battery = None self.battery = None
print("⚠️ ALBattery service not found")
self.video = None self.video = None
self.video_client = None self.video_client = None
self.running = True
# Audio setup variables
self.audio_device = None
self.audio_service_name = "SoundReceiver"
self.audio_device_index = audio_device_index # which laptop output device to play the mic feed on
self.running = True
self.head_yaw = 0.0 self.head_yaw = 0.0
self.head_pitch = 0.0 self.head_pitch = 0.0
# Battery tracking
self.battery_level = 100 self.battery_level = 100
self.last_batt_check = 0 self.last_batt_check = 0
# Volume of the NAO's own onboard speakers (0-100), controlled via
# ALAudioDevice.setOutputVolume. This is separate from - and has
# nothing to do with - the laptop playback of the robot's mic feed.
self.volume = 50
# Chatbox variables
self.typing_mode = False
self.chat_message = ""
# Get ALAudioDevice up front - this is needed for speaker volume
# control regardless of whether PyAudio/mic-listening is available.
self._init_audio_device()
# Initialize Video
self._init_video_maxfps() self._init_video_maxfps()
# Initialize mic listening (PyAudio -> laptop speakers)
if HAS_AUDIO:
self._init_mic_stream()
# Initialize Pygame
pygame.init() pygame.init()
self.screen = pygame.display.set_mode((320, 240)) self.screen = pygame.display.set_mode((320, 240))
pygame.display.set_caption("NAO Teleop | Battery: Checking...") pygame.display.set_caption("NAO Teleop | Battery: Checking...")
self.font = pygame.font.SysFont(None, 24)
self.clock = pygame.time.Clock() self.clock = pygame.time.Clock()
self.update_title()
def _init_audio_device(self):
try:
self.audio_device = self.session.service("ALAudioDevice")
self.volume = self.audio_device.getOutputVolume()
print(f"🔊 NAO speaker volume: {self.volume}%")
except Exception as e:
print(f"⚠️ Could not reach ALAudioDevice for speaker volume control: {e}")
self.audio_device = None
def _init_video_maxfps(self): def _init_video_maxfps(self):
try: try:
@@ -48,6 +248,29 @@ class NaoTeleop:
except: except:
print("❌ Camera not available") print("❌ Camera not available")
def _init_mic_stream(self):
if not self.audio_device:
print("❌ Can't start mic listening - ALAudioDevice unavailable")
return
try:
self.sound_receiver = SoundReceiver(output_device_index=self.audio_device_index)
# 1. Register the local service
self.session.registerService(self.audio_service_name, self.sound_receiver)
# 2. Add a tiny delay to let the network handshake complete
time.sleep(0.5)
# 3. Use the IP of the machine running the code
# We explicitly tell the robot the IP of your laptop (replace with your laptop IP if still failing)
# '0' tells it to auto-discover, but sometimes we need to be explicit.
self.audio_device.setClientPreferences(self.audio_service_name, 16000, 3, 0)
self.audio_device.subscribe(self.audio_service_name)
print("✅ Live Audio Stream ready")
except Exception as e:
print(f"❌ Audio init failed: {e}")
def get_image(self): def get_image(self):
if not self.video or not self.video_client: if not self.video or not self.video_client:
return None return None
@@ -63,112 +286,153 @@ class NaoTeleop:
return None return None
def wave_gesture(self): def wave_gesture(self):
if self.typing_mode: return
print("🤚 Waving hello...") print("🤚 Waving hello...")
try: try:
# Stiffen arm for accurate wave
self.motion.setStiffnesses("RArm", 1.0) self.motion.setStiffnesses("RArm", 1.0)
# Use all 6 joints to prevent xAssertArraySize crash
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"] names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
angles = [ angles = [
[-1.5, -1.5, -1.5, -1.5, -1.5], # RShoulderPitch (high up) [-1.5, -1.5, -1.5, -1.5, -1.5],
[-0.4, -0.7, -0.4, -0.7, -0.4], # RShoulderRoll [-0.4, -0.7, -0.4, -0.7, -0.4],
[ 0.6, 1.1, 0.6, 1.1, 0.6], # RElbowYaw [ 0.6, 1.1, 0.6, 1.1, 0.6],
[ 0.8, 0.3, 0.8, 0.3, 0.8], # RElbowRoll (proper bending) [ 0.8, 0.3, 0.8, 0.3, 0.8],
[ 0.0, 1.5, 0.0, -1.5, 0.0], # RWristYaw (the wave) [ 0.0, 1.5, 0.0, -1.5, 0.0],
[ 1.0, 1.0, 1.0, 1.0, 1.0] # RHand (keep open) [ 1.0, 1.0, 1.0, 1.0, 1.0]
] ]
times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 6 times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 6
self.motion.angleInterpolation(names, angles, times, True) self.motion.angleInterpolation(names, angles, times, True)
# Safely lower arm to true neutral
neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0] neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3) self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3)
# Relax the arm's stiffness to let the motors cool and walk naturally
self.motion.setStiffnesses("RArm", 0.6) self.motion.setStiffnesses("RArm", 0.6)
print("✅ Wave completed")
print("✅ Proper wave completed and arm relaxed")
except Exception as e: except Exception as e:
print(f"Wave error: {e}") print(f"Wave error: {e}")
def handle_head_movement(self): def handle_head_movement(self):
if self.typing_mode: return
keys = pygame.key.get_pressed() keys = pygame.key.get_pressed()
speed = 0.3 speed = 0.3
changed = False changed = False
if keys[pygame.K_i]: if keys[pygame.K_i]: self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True
self.head_pitch = max(self.head_pitch - speed, -0.5); changed = True if keys[pygame.K_k]: self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True
if keys[pygame.K_k]: if keys[pygame.K_j]: self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True
self.head_pitch = min(self.head_pitch + speed, 0.5); changed = True if keys[pygame.K_l]: self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True
if keys[pygame.K_j]:
self.head_yaw = min(self.head_yaw + speed, 2.0); changed = True
if keys[pygame.K_l]:
self.head_yaw = max(self.head_yaw - speed, -2.0); changed = True
if changed: if changed:
self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3) self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)
def check_battery(self): def update_title(self):
# Only ping the robot for battery data once every 10 seconds to save FPS mode = "[TYPING] " if self.typing_mode else ""
if not self.battery: pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | NAO Vol: {self.volume}%")
return
def change_volume(self, delta):
self.volume = int(min(100, max(0, self.volume + delta)))
if self.audio_device:
try:
self.audio_device.setOutputVolume(self.volume)
except Exception as e:
print(f"⚠️ Failed to set NAO speaker volume: {e}")
print(f"🔊 NAO speaker volume: {self.volume}%")
self.update_title()
def check_battery(self):
if not self.battery: return
now = time.time() now = time.time()
if now - self.last_batt_check > 10: if now - self.last_batt_check > 10:
try: try:
self.battery_level = self.battery.getBatteryCharge() self.battery_level = self.battery.getBatteryCharge()
pygame.display.set_caption(f"NAO Teleop | Battery: {self.battery_level}%")
except: except:
pass pass
self.update_title()
self.last_batt_check = now self.last_batt_check = now
def run(self): def run(self):
self.motion.wakeUp() self.motion.wakeUp()
self.posture.goToPosture("StandInit", 0.5) self.posture.goToPosture("StandInit", 0.5)
# Turn natural arm swinging back on so his walk looks normal! # Settle delay for carpet
time.sleep(1.0)
self.motion.setMoveArmsEnabled(True, True) self.motion.setMoveArmsEnabled(True, True)
print("🎮 Controls:") carpet_config = [
["StepHeight", 0.028],
["MaxStepFrequency", 0.6],
["TorsoWy", 0.05]
]
print("\n🎮 Controls:")
print(" WASD / Arrows = Walk") print(" WASD / Arrows = Walk")
print(" I J K L = Head") print(" I J K L = Head")
print(" 1 = Wave") print(" 1 = Wave")
print(" T = Type to Speak (TTS)")
print(" - / = = NAO speaker volume down / up")
print(" 0 = Mute NAO speaker")
print(" ESC = Quit") print(" ESC = Quit")
while self.running: while self.running:
# Zero-impact battery checker
self.check_battery() self.check_battery()
for event in pygame.event.get(): for event in pygame.event.get():
if event.type == pygame.QUIT: if event.type == pygame.QUIT:
self.running = False self.running = False
elif event.type == pygame.KEYDOWN: elif event.type == pygame.KEYDOWN:
# --- CHAT / TTS MODE ---
if self.typing_mode:
if event.key == pygame.K_RETURN:
if self.chat_message.strip():
print(f"🗣️ NAO saying: {self.chat_message}")
# Fire TTS in a background thread so the camera doesn't freeze
threading.Thread(target=self.tts.say, args=(self.chat_message,)).start()
self.chat_message = ""
self.typing_mode = False
# Force update title bar
self.last_batt_check = 0
elif event.key == pygame.K_ESCAPE:
self.chat_message = ""
self.typing_mode = False
self.last_batt_check = 0
elif event.key == pygame.K_BACKSPACE:
self.chat_message = self.chat_message[:-1]
else:
self.chat_message += event.unicode
# --- NORMAL DRIVING MODE ---
else:
if event.key == pygame.K_ESCAPE: if event.key == pygame.K_ESCAPE:
self.running = False self.running = False
elif event.key == pygame.K_1: elif event.key == pygame.K_1:
self.wave_gesture() self.wave_gesture()
elif event.key == pygame.K_t:
self.typing_mode = True
self.motion.stopMove()
self.last_batt_check = 0
elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS):
self.change_volume(-5)
elif event.key in (pygame.K_EQUALS, pygame.K_PLUS, pygame.K_KP_PLUS):
self.change_volume(5)
elif event.key == pygame.K_0:
self.change_volume(-self.volume)
# Walking # Walking (Only if not typing)
if not self.typing_mode:
keys = pygame.key.get_pressed() keys = pygame.key.get_pressed()
x = y = theta = 0.0 x = y = theta = 0.0
if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.9 if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.6
if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.9 if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.6
if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.7 if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.5
if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.7 if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.5
if abs(x) > 0.1 or abs(theta) > 0.1: if abs(x) > 0.1 or abs(theta) > 0.1:
self.motion.moveToward(x, y, theta) self.motion.moveToward(x, y, theta, carpet_config)
else: else:
self.motion.stopMove() self.motion.stopMove()
self.handle_head_movement() self.handle_head_movement()
# Camera # Camera Render
frame = self.get_image() frame = self.get_image()
if frame is not None: if frame is not None:
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
@@ -177,24 +441,51 @@ class NaoTeleop:
else: else:
self.screen.fill((10, 10, 30)) self.screen.fill((10, 10, 30))
# Chatbox Render
if self.typing_mode:
s = pygame.Surface((320, 40))
s.set_alpha(180)
s.fill((0, 0, 0))
self.screen.blit(s, (0, 200))
text_surface = self.font.render(f"Say: {self.chat_message}", True, (255, 255, 255))
self.screen.blit(text_surface, (10, 210))
pygame.display.flip() pygame.display.flip()
self.clock.tick(0) self.clock.tick(0)
# Shutdown # Shutdown sequence
print("🛑 Shutting down...") print("🛑 Shutting down...")
self.motion.stopMove() self.motion.stopMove()
self.motion.rest()
# Clean up Audio
if self.audio_device:
try:
self.audio_device.unsubscribe(self.audio_service_name)
except:
pass
if getattr(self, "sound_receiver", None):
self.sound_receiver.close()
# Clean up Video
if self.video and self.video_client: if self.video and self.video_client:
try: try:
self.video.unsubscribe(self.video_client) self.video.unsubscribe(self.video_client)
except: except:
pass pass
self.motion.rest()
pygame.quit() pygame.quit()
if __name__ == "__main__": if __name__ == "__main__":
parser = argparse.ArgumentParser() parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1") parser.add_argument("--ip", type=str, default="127.0.0.1")
parser.add_argument("--port", type=int, default=9559) parser.add_argument("--port", type=int, default=9559)
parser.add_argument("--audio-device-index", type=int, default=None,
help="Force a specific PyAudio output device index for the robot's "
"mic feed (see the '🔈 Available output devices' list printed "
"at startup) if the system default isn't your speakers.")
args = parser.parse_args() args = parser.parse_args()
session = qi.Session() session = qi.Session()
@@ -205,4 +496,4 @@ if __name__ == "__main__":
print(f"❌ Connection failed: {e}") print(f"❌ Connection failed: {e}")
sys.exit(1) sys.exit(1)
NaoTeleop(session).run() NaoTeleop(session, audio_device_index=args.audio_device_index).run()