luccapirovano/naowalk
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

amplified the audio stream and added a --mic-gain parameter that defaults to 8

Browse Source
This commit is contained in:
Lucca Pirovano
2026-07-15 17:11:02 -04:00
parent 7cc738fba7
commit e943073717
1 changed files with 59 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
naowalk.py
+59 -75
View File
@@ -11,7 +11,7 @@ import cv2
import numpy as np import numpy as np
import pygame import pygame
# --- NEW: Audio Library --- # --- Audio Library ---
try: try:
import pyaudio import pyaudio
HAS_AUDIO = True HAS_AUDIO = True
@@ -21,7 +21,7 @@ except ImportError:
# ========================================== # ==========================================
# AUDIO RECEIVER SERVICE (Runs in background) # AUDIO RECEIVER
# ========================================== # ==========================================
class SoundReceiver: class SoundReceiver:
PREBUFFER_CHUNKS = 3 PREBUFFER_CHUNKS = 3
@@ -34,36 +34,26 @@ class SoundReceiver:
self._write_errors = 0 self._write_errors = 0
self._peak_since_print = 0 self._peak_since_print = 0
self._last_status_print = 0 self._last_status_print = 0
# NEW: Amplification (higher = louder)
self.mic_gain = float(mic_gain) self.mic_gain = float(mic_gain)
if HAS_AUDIO: if HAS_AUDIO:
self.p = pyaudio.PyAudio() self.p = pyaudio.PyAudio()
print("🔈 Available output devices:") print("🔈 Available output devices:")
for i in range(self.p.get_device_count()): for i in range(self.p.get_device_count()):
info = self.p.get_device_info_by_index(i) info = self.p.get_device_info_by_index(i)
if info.get("maxOutputChannels", 0) > 0: if info.get("maxOutputChannels", 0) > 0:
print(f" [{i}] {info['name']}") print(f" [{i}] {info['name']}")
try: try:
chosen = (self.p.get_device_info_by_index(output_device_index) chosen = (self.p.get_device_info_by_index(output_device_index) if output_device_index is not None
if output_device_index is not None
else self.p.get_default_output_device_info()) else self.p.get_default_output_device_info())
print(f"🔈 Using output device: [{chosen['index']}] {chosen['name']}" 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: except Exception as e:
print(f"⚠️ Could not resolve output device: {e}") print(f"⚠️ Could not resolve output device: {e}")
self.stream = self.p.open(format=pyaudio.paInt16, self.stream = self.p.open(format=pyaudio.paInt16, channels=1, rate=16000,
channels=1, output=True, output_device_index=output_device_index,
rate=16000, frames_per_buffer=2048)
output=True,
output_device_index=output_device_index,
frames_per_buffer=2048)
self._play_test_tone() self._play_test_tone()
self.queue = queue.Queue(maxsize=40) self.queue = queue.Queue(maxsize=40)
self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True) self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True)
self.playback_thread.start() self.playback_thread.start()
@@ -74,13 +64,12 @@ class SoundReceiver:
t = np.linspace(0, duration, int(rate * duration), False) t = np.linspace(0, duration, int(rate * duration), False)
tone = (np.sin(freq * t * 2 * np.pi) * 12000).astype(np.int16) tone = (np.sin(freq * t * 2 * np.pi) * 12000).astype(np.int16)
self.stream.write(tone.tobytes()) self.stream.write(tone.tobytes())
print("🔔 Played a test beep. If you didn't hear it, fix laptop audio first.") print("🔔 Test beep played.")
except Exception as e: except Exception as e:
print(f"⚠️ Test tone failed: {e}") print(f"⚠️ Test tone failed: {e}")
def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
if not HAS_AUDIO: if not HAS_AUDIO: return
return
self.chunks_received += 1 self.chunks_received += 1
try: try:
self.queue.put_nowait(bytes(buffer)) self.queue.put_nowait(bytes(buffer))
@@ -121,18 +110,13 @@ class SoundReceiver:
def _write_chunk(self, raw_bytes): def _write_chunk(self, raw_bytes):
try: try:
# === AMPLIFICATION ===
samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32) samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32)
if samples.size: if samples.size:
amplified = samples * self.mic_gain amplified = np.clip(samples * self.mic_gain, -32768, 32767)
amplified = np.clip(amplified, -32768, 32767) # prevent clipping distortion
boosted_bytes = amplified.astype(np.int16).tobytes()
peak = int(np.abs(amplified).max()) peak = int(np.abs(amplified).max())
if peak > self._peak_since_print: if peak > self._peak_since_print:
self._peak_since_print = peak self._peak_since_print = peak
self.stream.write(amplified.astype(np.int16).tobytes())
self.stream.write(boosted_bytes)
else: else:
self.stream.write(raw_bytes) self.stream.write(raw_bytes)
except Exception as e: except Exception as e:
@@ -143,23 +127,20 @@ class SoundReceiver:
def close(self): def close(self):
self.running = False self.running = False
if HAS_AUDIO: if HAS_AUDIO:
try: try: self.playback_thread.join(timeout=1.0)
self.playback_thread.join(timeout=1.0) except: pass
except Exception:
pass
try: try:
self.stream.stop_stream() self.stream.stop_stream()
self.stream.close() self.stream.close()
self.p.terminate() self.p.terminate()
except Exception: except: pass
pass
# ========================================== # ==========================================
# MAIN TELEOP CLASS # MAIN TELEOP CLASS
# ========================================== # ==========================================
class NaoTeleop: class NaoTeleop:
def __init__(self, session, audio_device_index=None, mic_channel=1, mic_gain=8.0): def __init__(self, session, audio_device_index=None, mic_channel=1, mic_gain=8.0, video_scale=2.0):
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")
@@ -177,27 +158,29 @@ class NaoTeleop:
self.audio_service_name = "SoundReceiver" self.audio_service_name = "SoundReceiver"
self.audio_device_index = audio_device_index self.audio_device_index = audio_device_index
self.mic_channel = mic_channel self.mic_channel = mic_channel
self.mic_gain = mic_gain # NEW self.mic_gain = mic_gain
self.running = True self.running = True
self.head_yaw = 0.0 self.head_yaw = 0.0
self.head_pitch = 0.0 self.head_pitch = 0.0
self.battery_level = 100 self.battery_level = 100
self.last_batt_check = 0 self.last_batt_check = 0
self.volume = 50 self.volume = 50
self.typing_mode = False self.typing_mode = False
self.chat_message = "" self.chat_message = ""
self.video_scale = video_scale
self.display_width = int(320 * video_scale)
self.display_height = int(240 * video_scale)
self._init_audio_device() self._init_audio_device()
self._init_video_maxfps() self._init_video_maxfps()
if HAS_AUDIO: if HAS_AUDIO:
self._init_mic_stream() self._init_mic_stream()
pygame.init() pygame.init()
self.screen = pygame.display.set_mode((320, 240)) self.screen = pygame.display.set_mode((self.display_width, self.display_height))
pygame.display.set_caption("NAO Teleop | Battery: Checking...") pygame.display.set_caption("NAO Teleop | Battery: Checking...")
self.font = pygame.font.SysFont(None, 24) self.font = pygame.font.SysFont(None, 24)
self.clock = pygame.time.Clock() self.clock = pygame.time.Clock()
@@ -222,24 +205,18 @@ class NaoTeleop:
def _init_mic_stream(self): def _init_mic_stream(self):
if not self.audio_device: if not self.audio_device:
print("❌ Can't start mic listening - ALAudioDevice unavailable") print("❌ Can't start mic listening")
return return
try: try:
self.sound_receiver = SoundReceiver( self.sound_receiver = SoundReceiver(output_device_index=self.audio_device_index, mic_gain=self.mic_gain)
output_device_index=self.audio_device_index,
mic_gain=self.mic_gain
)
self.session.registerService(self.audio_service_name, self.sound_receiver) self.session.registerService(self.audio_service_name, self.sound_receiver)
time.sleep(0.5) time.sleep(0.5)
self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0) self.audio_device.setClientPreferences(self.audio_service_name, 16000, self.mic_channel, 0)
self.audio_device.subscribe(self.audio_service_name) self.audio_device.subscribe(self.audio_service_name)
print(f"✅ Live Audio Stream ready (gain = {self.mic_gain}x)")
print(f"✅ Live Audio Stream ready (mic gain = {self.mic_gain}x)")
except Exception as e: except Exception as e:
print(f"❌ Audio init failed: {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
@@ -254,6 +231,13 @@ class NaoTeleop:
pass pass
return None return None
def enhance_frame(self, frame):
"""Simple fast upscale"""
if frame is None:
return None
return cv2.resize(frame, (self.display_width, self.display_height),
interpolation=cv2.INTER_CUBIC)
def wave_gesture(self): def wave_gesture(self):
if self.typing_mode: return if self.typing_mode: return
print("🤚 Waving hello...") print("🤚 Waving hello...")
@@ -292,6 +276,12 @@ class NaoTeleop:
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 reset_head(self):
self.head_yaw = 0.0
self.head_pitch = 0.0
self.motion.setAngles(["HeadYaw", "HeadPitch"], [0.0, 0.0], 0.4)
print("Head reset to neutral")
def update_title(self): def update_title(self):
mode = "[TYPING] " if self.typing_mode else "" mode = "[TYPING] " if self.typing_mode else ""
pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | NAO Vol: {self.volume}%") pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | NAO Vol: {self.volume}%")
@@ -320,22 +310,18 @@ class NaoTeleop:
def run(self): def run(self):
self.motion.wakeUp() self.motion.wakeUp()
self.posture.goToPosture("StandInit", 0.5) self.posture.goToPosture("StandInit", 0.5)
time.sleep(1.0) time.sleep(1.0)
self.motion.setMoveArmsEnabled(True, True) self.motion.setMoveArmsEnabled(True, True)
carpet_config = [ carpet_config = [["StepHeight", 0.028], ["MaxStepFrequency", 0.6], ["TorsoWy", 0.05]]
["StepHeight", 0.028],
["MaxStepFrequency", 0.6],
["TorsoWy", 0.05]
]
print("\n🎮 Controls:") print("\n🎮 Controls:")
print(" WASD / Arrows = Walk") print(" WASD / Arrows = Walk")
print(" I J K L = Head") print(" I J K L = Head")
print(" SPACE = Reset head to neutral")
print(" 1 = Wave") print(" 1 = Wave")
print(" T = Type to Speak (TTS)") print(" T = Type to Speak (TTS)")
print(" - / = = NAO speaker volume down / up") print(" - / = = NAO speaker volume")
print(" 0 = Mute NAO speaker") print(" 0 = Mute NAO speaker")
print(" ESC = Quit") print(" ESC = Quit")
@@ -367,6 +353,8 @@ class NaoTeleop:
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_SPACE:
self.reset_head()
elif event.key == pygame.K_t: elif event.key == pygame.K_t:
self.typing_mode = True self.typing_mode = True
self.motion.stopMove() self.motion.stopMove()
@@ -395,40 +383,34 @@ class NaoTeleop:
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) enhanced = self.enhance_frame(frame)
rgb = cv2.cvtColor(enhanced, cv2.COLOR_BGR2RGB)
surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1)) surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1))
self.screen.blit(surf, (0, 0)) self.screen.blit(surf, (0, 0))
else: else:
self.screen.fill((10, 10, 30)) self.screen.fill((10, 10, 30))
if self.typing_mode: if self.typing_mode:
s = pygame.Surface((320, 40)) s = pygame.Surface((self.display_width, 40))
s.set_alpha(180) s.set_alpha(180)
s.fill((0, 0, 0)) s.fill((0, 0, 0))
self.screen.blit(s, (0, 200)) self.screen.blit(s, (0, self.display_height - 40))
text_surface = self.font.render(f"Say: {self.chat_message}", True, (255, 255, 255)) text = self.font.render(f"Say: {self.chat_message}", True, (255, 255, 255))
self.screen.blit(text_surface, (10, 210)) self.screen.blit(text, (10, self.display_height - 30))
pygame.display.flip() pygame.display.flip()
self.clock.tick(0) self.clock.tick(0)
print("🛑 Shutting down...") print("🛑 Shutting down...")
self.motion.stopMove() self.motion.stopMove()
if self.audio_device: if self.audio_device:
try: try: self.audio_device.unsubscribe(self.audio_service_name)
self.audio_device.unsubscribe(self.audio_service_name) except: pass
except:
pass
if getattr(self, "sound_receiver", None): if getattr(self, "sound_receiver", None):
self.sound_receiver.close() self.sound_receiver.close()
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: pass
except:
pass
self.motion.rest() self.motion.rest()
pygame.quit() pygame.quit()
@@ -441,8 +423,8 @@ if __name__ == "__main__":
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) parser.add_argument("--audio-device-index", type=int, default=None)
parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left") parser.add_argument("--mic-channel", choices=MIC_CHANNELS.keys(), default="left")
parser.add_argument("--mic-gain", type=float, default=8.0, parser.add_argument("--mic-gain", type=float, default=8.0)
help="Microphone amplification (try 6-12). Higher = louder.") parser.add_argument("--video-scale", type=float, default=2.0)
args = parser.parse_args() args = parser.parse_args()
session = qi.Session() session = qi.Session()
@@ -453,6 +435,8 @@ if __name__ == "__main__":
print(f"❌ Connection failed: {e}") print(f"❌ Connection failed: {e}")
sys.exit(1) sys.exit(1)
NaoTeleop(session, audio_device_index=args.audio_device_index, NaoTeleop(session,
audio_device_index=args.audio_device_index,
mic_channel=MIC_CHANNELS[args.mic_channel], mic_channel=MIC_CHANNELS[args.mic_channel],
mic_gain=args.mic_gain).run() mic_gain=args.mic_gain,
video_scale=args.video_scale).run()