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
+57 -73
View File
@@ -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,
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,21 +205,15 @@ 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}")
@@ -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()