amplified the audio stream and added a --mic-gain parameter that defaults to 8
This commit is contained in:
Lucca Pirovano
+59
-75
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user