From e94307371782438f2f1262e430fbab446cd18434 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 17:11:02 -0400 Subject: [PATCH] amplified the audio stream and added a --mic-gain parameter that defaults to 8 --- naowalk.py | 134 +++++++++++++++++++++++------------------------------ 1 file changed, 59 insertions(+), 75 deletions(-) diff --git a/naowalk.py b/naowalk.py index 09cac82..09f7aaf 100644 --- a/naowalk.py +++ b/naowalk.py @@ -11,7 +11,7 @@ import cv2 import numpy as np import pygame -# --- NEW: Audio Library --- +# --- Audio Library --- try: import pyaudio HAS_AUDIO = True @@ -21,7 +21,7 @@ except ImportError: # ========================================== -# AUDIO RECEIVER SERVICE (Runs in background) +# AUDIO RECEIVER # ========================================== class SoundReceiver: PREBUFFER_CHUNKS = 3 @@ -34,36 +34,26 @@ class SoundReceiver: self._write_errors = 0 self._peak_since_print = 0 self._last_status_print = 0 - - # NEW: Amplification (higher = louder) self.mic_gain = float(mic_gain) if HAS_AUDIO: self.p = pyaudio.PyAudio() - 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 + 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)'}") + print(f"šŸ”ˆ Using output device: [{chosen['index']}] {chosen['name']}") except Exception as e: print(f"āš ļø Could not resolve output device: {e}") - 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.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() - self.queue = queue.Queue(maxsize=40) self.playback_thread = threading.Thread(target=self._playback_loop, daemon=True) self.playback_thread.start() @@ -74,13 +64,12 @@ class SoundReceiver: 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, fix laptop audio first.") + print("šŸ”” Test beep played.") except Exception as e: print(f"āš ļø Test tone failed: {e}") def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer): - if not HAS_AUDIO: - return + if not HAS_AUDIO: return self.chunks_received += 1 try: self.queue.put_nowait(bytes(buffer)) @@ -121,18 +110,13 @@ class SoundReceiver: def _write_chunk(self, raw_bytes): try: - # === AMPLIFICATION === samples = np.frombuffer(raw_bytes, dtype=np.int16).astype(np.float32) if samples.size: - amplified = samples * self.mic_gain - amplified = np.clip(amplified, -32768, 32767) # prevent clipping distortion - boosted_bytes = amplified.astype(np.int16).tobytes() - + amplified = np.clip(samples * self.mic_gain, -32768, 32767) peak = int(np.abs(amplified).max()) if peak > self._peak_since_print: self._peak_since_print = peak - - self.stream.write(boosted_bytes) + self.stream.write(amplified.astype(np.int16).tobytes()) else: self.stream.write(raw_bytes) except Exception as e: @@ -143,23 +127,20 @@ class SoundReceiver: def close(self): self.running = False if HAS_AUDIO: - try: - self.playback_thread.join(timeout=1.0) - except Exception: - pass + try: self.playback_thread.join(timeout=1.0) + except: pass try: self.stream.stop_stream() self.stream.close() self.p.terminate() - except Exception: - pass + except: pass # ========================================== # MAIN TELEOP CLASS # ========================================== 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.motion = session.service("ALMotion") self.posture = session.service("ALRobotPosture") @@ -177,27 +158,29 @@ class NaoTeleop: self.audio_service_name = "SoundReceiver" self.audio_device_index = audio_device_index self.mic_channel = mic_channel - self.mic_gain = mic_gain # NEW + self.mic_gain = mic_gain self.running = True self.head_yaw = 0.0 self.head_pitch = 0.0 self.battery_level = 100 self.last_batt_check = 0 - self.volume = 50 self.typing_mode = False 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_video_maxfps() - if HAS_AUDIO: self._init_mic_stream() 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...") self.font = pygame.font.SysFont(None, 24) self.clock = pygame.time.Clock() @@ -222,24 +205,18 @@ class NaoTeleop: def _init_mic_stream(self): if not self.audio_device: - print("āŒ Can't start mic listening - ALAudioDevice unavailable") + print("āŒ Can't start mic listening") return try: - self.sound_receiver = SoundReceiver( - output_device_index=self.audio_device_index, - mic_gain=self.mic_gain - ) - + self.sound_receiver = SoundReceiver(output_device_index=self.audio_device_index, mic_gain=self.mic_gain) 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"āœ… Live Audio Stream ready (mic gain = {self.mic_gain}x)") + print(f"āœ… Live Audio Stream ready (gain = {self.mic_gain}x)") 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 @@ -254,6 +231,13 @@ class NaoTeleop: pass 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): if self.typing_mode: return print("šŸ¤š Waving hello...") @@ -292,6 +276,12 @@ class NaoTeleop: if changed: 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): mode = "[TYPING] " if self.typing_mode else "" 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): self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) - time.sleep(1.0) self.motion.setMoveArmsEnabled(True, True) - carpet_config = [ - ["StepHeight", 0.028], - ["MaxStepFrequency", 0.6], - ["TorsoWy", 0.05] - ] + 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(" SPACE = Reset head to neutral") print(" 1 = Wave") print(" T = Type to Speak (TTS)") - print(" - / = = NAO speaker volume down / up") + print(" - / = = NAO speaker volume") print(" 0 = Mute NAO speaker") print(" ESC = Quit") @@ -367,6 +353,8 @@ class NaoTeleop: self.running = False elif event.key == pygame.K_1: self.wave_gesture() + elif event.key == pygame.K_SPACE: + self.reset_head() elif event.key == pygame.K_t: self.typing_mode = True self.motion.stopMove() @@ -395,40 +383,34 @@ class NaoTeleop: frame = self.get_image() 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)) self.screen.blit(surf, (0, 0)) else: self.screen.fill((10, 10, 30)) if self.typing_mode: - s = pygame.Surface((320, 40)) + s = pygame.Surface((self.display_width, 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)) + self.screen.blit(s, (0, self.display_height - 40)) + text = self.font.render(f"Say: {self.chat_message}", True, (255, 255, 255)) + self.screen.blit(text, (10, self.display_height - 30)) pygame.display.flip() self.clock.tick(0) print("šŸ›‘ Shutting down...") self.motion.stopMove() - if self.audio_device: - try: - self.audio_device.unsubscribe(self.audio_service_name) - except: - pass + try: self.audio_device.unsubscribe(self.audio_service_name) + except: pass if getattr(self, "sound_receiver", None): self.sound_receiver.close() - if self.video and self.video_client: - try: - self.video.unsubscribe(self.video_client) - except: - pass - + try: self.video.unsubscribe(self.video_client) + except: pass self.motion.rest() pygame.quit() @@ -441,8 +423,8 @@ if __name__ == "__main__": parser.add_argument("--port", type=int, default=9559) 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-gain", type=float, default=8.0, - help="Microphone amplification (try 6-12). Higher = louder.") + parser.add_argument("--mic-gain", type=float, default=8.0) + parser.add_argument("--video-scale", type=float, default=2.0) args = parser.parse_args() session = qi.Session() @@ -453,6 +435,8 @@ if __name__ == "__main__": print(f"āŒ Connection failed: {e}") 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_gain=args.mic_gain).run() + mic_gain=args.mic_gain, + video_scale=args.video_scale).run()