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/revivalstack/ai-chat-exporter
+342 -51
View File
@@ -5,40 +5,240 @@ import qi
import argparse
import sys
import time
import pygame
import threading
import queue
import cv2
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:
def __init__(self, session):
def __init__(self, session, audio_device_index=None):
self.session = session
self.motion = session.service("ALMotion")
self.posture = session.service("ALRobotPosture")
self.tts = session.service("ALTextToSpeech")
# New: Hook into the battery service
try:
self.battery = session.service("ALBattery")
except:
self.battery = None
print("⚠️ ALBattery service not found")
self.video = 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_pitch = 0.0
# Battery tracking
self.battery_level = 100
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()
# Initialize mic listening (PyAudio -> laptop speakers)
if HAS_AUDIO:
self._init_mic_stream()
# Initialize Pygame
pygame.init()
self.screen = pygame.display.set_mode((320, 240))
pygame.display.set_caption("NAO Teleop | Battery: Checking...")
self.font = pygame.font.SysFont(None, 24)
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):
try:
@@ -48,6 +248,29 @@ class NaoTeleop:
except:
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):
if not self.video or not self.video_client:
return None
@@ -63,112 +286,153 @@ class NaoTeleop:
return None
def wave_gesture(self):
if self.typing_mode: return
print("🤚 Waving hello...")
try:
# Stiffen arm for accurate wave
self.motion.setStiffnesses("RArm", 1.0)
# Use all 6 joints to prevent xAssertArraySize crash
names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
angles = [
[-1.5, -1.5, -1.5, -1.5, -1.5], # RShoulderPitch (high up)
[-0.4, -0.7, -0.4, -0.7, -0.4], # RShoulderRoll
[ 0.6, 1.1, 0.6, 1.1, 0.6], # RElbowYaw
[ 0.8, 0.3, 0.8, 0.3, 0.8], # RElbowRoll (proper bending)
[ 0.0, 1.5, 0.0, -1.5, 0.0], # RWristYaw (the wave)
[ 1.0, 1.0, 1.0, 1.0, 1.0] # RHand (keep open)
[-1.5, -1.5, -1.5, -1.5, -1.5],
[-0.4, -0.7, -0.4, -0.7, -0.4],
[ 0.6, 1.1, 0.6, 1.1, 0.6],
[ 0.8, 0.3, 0.8, 0.3, 0.8],
[ 0.0, 1.5, 0.0, -1.5, 0.0],
[ 1.0, 1.0, 1.0, 1.0, 1.0]
]
times = [[0.5, 1.0, 1.5, 2.0, 2.5]] * 6
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]
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)
print("✅ Proper wave completed and arm relaxed")
print("✅ Wave completed")
except Exception as e:
print(f"Wave error: {e}")
def handle_head_movement(self):
if self.typing_mode: return
keys = pygame.key.get_pressed()
speed = 0.3
changed = False
if keys[pygame.K_i]:
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_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 keys[pygame.K_i]: 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_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:
self.motion.setAngles(["HeadYaw", "HeadPitch"], [self.head_yaw, self.head_pitch], 0.3)
def check_battery(self):
# Only ping the robot for battery data once every 10 seconds to save FPS
if not self.battery:
return
def update_title(self):
mode = "[TYPING] " if self.typing_mode else ""
pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | NAO Vol: {self.volume}%")
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()
if now - self.last_batt_check > 10:
try:
self.battery_level = self.battery.getBatteryCharge()
pygame.display.set_caption(f"NAO Teleop | Battery: {self.battery_level}%")
except:
pass
self.update_title()
self.last_batt_check = now
def run(self):
self.motion.wakeUp()
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)
print("🎮 Controls:")
carpet_config = [
["StepHeight", 0.028],
["MaxStepFrequency", 0.6],
["TorsoWy", 0.05]
]
print("\n🎮 Controls:")
print(" WASD / Arrows = Walk")
print(" I J K L = Head")
print(" 1 = Wave")
print(" T = Type to Speak (TTS)")
print(" - / = = NAO speaker volume down / up")
print(" 0 = Mute NAO speaker")
print(" ESC = Quit")
while self.running:
# Zero-impact battery checker
self.check_battery()
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.running = False
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:
self.running = False
elif event.key == pygame.K_1:
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()
x = y = theta = 0.0
if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.9
if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.9
if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.7
if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.7
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.6
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.5
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:
self.motion.stopMove()
self.handle_head_movement()
# Camera
# Camera Render
frame = self.get_image()
if frame is not None:
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
@@ -177,24 +441,51 @@ class NaoTeleop:
else:
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()
self.clock.tick(0)
# Shutdown
# Shutdown sequence
print("🛑 Shutting down...")
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:
try:
self.video.unsubscribe(self.video_client)
except:
pass
self.motion.rest()
pygame.quit()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="127.0.0.1")
parser.add_argument("--port", type=int, default=9559)
parser.add_argument("--audio-device-index", type=int, default=None,
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()
session = qi.Session()
@@ -205,4 +496,4 @@ if __name__ == "__main__":
print(f"❌ Connection failed: {e}")
sys.exit(1)
NaoTeleop(session).run()
NaoTeleop(session, audio_device_index=args.audio_device_index).run()