added WIP for hearing the nao's mic through laptop and tts chat with T
This commit is contained in:
Lucca Pirovano
+154
-65
@@ -5,39 +5,87 @@ import qi
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
import pygame
|
||||
import threading
|
||||
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:
|
||||
def __init__(self):
|
||||
if HAS_AUDIO:
|
||||
self.p = pyaudio.PyAudio()
|
||||
# NAO front mic is usually 16000Hz, 1 channel, 16-bit
|
||||
self.stream = self.p.open(format=pyaudio.paInt16,
|
||||
channels=1,
|
||||
rate=16000,
|
||||
output=True)
|
||||
|
||||
# ALAudioDevice strictly requires this exact method signature to send data
|
||||
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
|
||||
if HAS_AUDIO and self.stream:
|
||||
try:
|
||||
# Play the raw audio bytes pushed from the robot directly to your speakers
|
||||
self.stream.write(bytes(buffer))
|
||||
except Exception as e:
|
||||
pass
|
||||
|
||||
|
||||
# ==========================================
|
||||
# MAIN TELEOP CLASS
|
||||
# ==========================================
|
||||
class NaoTeleop:
|
||||
def __init__(self, session):
|
||||
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.running = True
|
||||
self.head_yaw = 0.0
|
||||
self.head_pitch = 0.0
|
||||
|
||||
# Battery tracking
|
||||
self.battery_level = 100
|
||||
self.last_batt_check = 0
|
||||
|
||||
# Chatbox variables
|
||||
self.typing_mode = False
|
||||
self.chat_message = ""
|
||||
|
||||
# Initialize Video
|
||||
self._init_video_maxfps()
|
||||
|
||||
# Initialize Audio
|
||||
if HAS_AUDIO:
|
||||
self._init_audio()
|
||||
|
||||
# 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()
|
||||
|
||||
def _init_video_maxfps(self):
|
||||
@@ -48,6 +96,21 @@ class NaoTeleop:
|
||||
except:
|
||||
print("❌ Camera not available")
|
||||
|
||||
def _init_audio(self):
|
||||
try:
|
||||
self.audio_device = self.session.service("ALAudioDevice")
|
||||
# Register our local class as a service on the robot's network
|
||||
self.sound_receiver = SoundReceiver()
|
||||
self.session.registerService(self.audio_service_name, self.sound_receiver)
|
||||
|
||||
# Configure: 16000Hz, Channel 3 (Front Mic), 0 (Interleaved)
|
||||
self.audio_device.setClientPreferences(self.audio_service_name, 16000, 3, 0)
|
||||
self.audio_device.subscribe(self.audio_service_name)
|
||||
print("✅ Live Audio Stream ready (Check your speakers!)")
|
||||
except Exception as e:
|
||||
print(f"❌ Audio init failed: {e}")
|
||||
self.audio_device = None
|
||||
|
||||
def get_image(self):
|
||||
if not self.video or not self.video_client:
|
||||
return None
|
||||
@@ -63,66 +126,51 @@ 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
|
||||
|
||||
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}%")
|
||||
mode = "[TYPING] " if self.typing_mode else ""
|
||||
pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}%")
|
||||
except:
|
||||
pass
|
||||
self.last_batt_check = now
|
||||
@@ -131,25 +179,21 @@ class NaoTeleop:
|
||||
self.motion.wakeUp()
|
||||
self.posture.goToPosture("StandInit", 0.5)
|
||||
|
||||
# 1. SETTLE DELAY: Let his internal gyroscopes adapt to the squishy carpet
|
||||
# Settle delay for carpet
|
||||
time.sleep(1.0)
|
||||
|
||||
self.motion.setMoveArmsEnabled(True, True)
|
||||
|
||||
# 2. CARPET GAIT CONFIGURATION
|
||||
# StepHeight: Lifts the foot 2.8cm (higher than default) to clear fibers
|
||||
# MaxStepFrequency: 0.6 slows the step rhythm down for better balance
|
||||
# TorsoWy: 0.05 leans his torso just slightly forward for better momentum
|
||||
carpet_config = [
|
||||
["StepHeight", 0.028],
|
||||
["MaxStepFrequency", 0.6],
|
||||
["TorsoWy", 0.05]
|
||||
]
|
||||
|
||||
print("🎮 Controls:")
|
||||
print("\n🎮 Controls:")
|
||||
print(" WASD / Arrows = Walk")
|
||||
print(" I J K L = Head")
|
||||
print(" 1 = Wave")
|
||||
print(" T = Type to Speak (TTS)")
|
||||
print(" ESC = Quit")
|
||||
|
||||
while self.running:
|
||||
@@ -159,30 +203,55 @@ class NaoTeleop:
|
||||
if event.type == pygame.QUIT:
|
||||
self.running = False
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_ESCAPE:
|
||||
self.running = False
|
||||
elif event.key == pygame.K_1:
|
||||
self.wave_gesture()
|
||||
|
||||
# Walking
|
||||
keys = pygame.key.get_pressed()
|
||||
x = y = theta = 0.0
|
||||
# --- 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
|
||||
|
||||
# 3. SAFER TOP SPEED: Dropped from 0.9 (sprinting) to 0.6 (safe walking)
|
||||
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
|
||||
# --- 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
|
||||
|
||||
if abs(x) > 0.1 or abs(theta) > 0.1:
|
||||
# Pass the custom carpet gait to the walk engine
|
||||
self.motion.moveToward(x, y, theta, carpet_config)
|
||||
else:
|
||||
self.motion.stopMove()
|
||||
# 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.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
|
||||
|
||||
self.handle_head_movement()
|
||||
if abs(x) > 0.1 or abs(theta) > 0.1:
|
||||
self.motion.moveToward(x, y, theta, carpet_config)
|
||||
else:
|
||||
self.motion.stopMove()
|
||||
|
||||
# Camera
|
||||
self.handle_head_movement()
|
||||
|
||||
# Camera Render
|
||||
frame = self.get_image()
|
||||
if frame is not None:
|
||||
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
||||
@@ -191,18 +260,38 @@ 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
|
||||
|
||||
# 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__":
|
||||
|
||||
Reference in New Issue
Block a user