added yt-dlp music player and readme.md

naowalk.py

@@ -10,18 +10,18 @@ import queue
 import cv2
 import numpy as np
 import pygame
+import subprocess
 
-# --- NEW: Audio Library ---
+# --- Audio for mic ---
 try:
     import pyaudio
     HAS_AUDIO = True
 except ImportError:
     HAS_AUDIO = False
-    print("⚠️ PyAudio not installed. Run 'pip install pyaudio' to hear the robot.")
+    print("⚠️ PyAudio not installed.")
-
 
 # ==========================================
-# AUDIO RECEIVER SERVICE (Runs in background)
+# NAO MIC STREAM TO LAPTOP
 # ==========================================
 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)
+                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())
-                print(f"🔈 Using output device: [{chosen['index']}] {chosen['name']}"
+                print(f"🔈 Using: [{chosen['index']}] {chosen['name']}")
-                      f"{' (forced via --audio-device-index)' if output_device_index is not None else ' (default)'}")
+            except:
-            except Exception as e:
+                pass
-                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,10 @@ 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.")
+        except: pass
-        except Exception as e:
-            print(f"⚠️ Test tone failed: {e}")
 
     def processRemote(self, nbOfChannels, nbOfSamplesByChannel, timeStamp, buffer):
-        if not HAS_AUDIO:
+        if not HAS_AUDIO: return
-            return
         self.chunks_received += 1
         try:
             self.queue.put_nowait(bytes(buffer))
@@ -88,8 +75,7 @@ class SoundReceiver:
             try:
                 self.queue.get_nowait()
                 self.queue.put_nowait(bytes(buffer))
-            except queue.Empty:
+            except: pass
-                pass
 
     def _playback_loop(self):
         primed = []
@@ -109,96 +95,162 @@ class SoundReceiver:
                 self.underrun_count += 1
                 try:
                     self.stream.write(self.SILENCE_CHUNK)
-                except Exception:
+                except: pass
-                    pass
 
             now = time.time()
             if now - self._last_status_print > 10:
                 self._last_status_print = now
-                print(f"🎤 audio: {self.chunks_received} chunks, {self.underrun_count} underruns, "
+                print(f"🎤 mic: {self.chunks_received} chunks, {self.underrun_count} underruns, peak {self._peak_since_print}")
-                      f"peak {self._peak_since_print}/32767 | gain={self.mic_gain}x")
                 self._peak_since_print = 0
 
     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(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())
                 if peak > self._peak_since_print:
                     self._peak_since_print = peak
-
+                self.stream.write(amplified.astype(np.int16).tobytes())
-                self.stream.write(boosted_bytes)
             else:
                 self.stream.write(raw_bytes)
-        except Exception as e:
+        except: pass
-            self._write_errors += 1
-            if self._write_errors <= 3:
-                print(f"⚠️ audio write error: {e}")
 
     def close(self):
         self.running = False
         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:
                 self.stream.stop_stream()
                 self.stream.close()
                 self.p.terminate()
-            except Exception:
+            except: pass
-                pass
 
 
 # ==========================================
-# MAIN TELEOP CLASS
+# MUSIC PLAYER ON NAO
+# ==========================================
+class NaoMusicPlayer:
+    def __init__(self, session):
+        self.session = session
+        self.player = session.service("ALAudioPlayer")
+        self.music_dir = "/home/nao/music"
+        try:
+            self.session.service("ALFileManager").createFolder(self.music_dir)
+        except:
+            pass
+        self.current_file = None
+        self.is_playing = False
+
+    def search_and_play(self, query):
+        print(f"🔍 Searching: {query}")
+        try:
+            result = subprocess.run(['ytfzf', '-m', '-L', query], capture_output=True, text=True, timeout=15)
+            url = result.stdout.strip().split('\n')[0] if result.stdout.strip() else None
+            if not url:
+                import yt_dlp
+                with yt_dlp.YoutubeDL({'quiet': True, 'extract_flat': True}) as ydl:
+                    info = ydl.extract_info(f"ytsearch1:{query}", download=False)
+                    url = info['entries'][0]['url']
+        except Exception as e:
+            print(f"Search failed: {e}")
+            return False
+
+        self.play_on_nao(url, query)
+        return True
+
+    def play_on_nao(self, url, title=""):
+        self.stop()
+        try:
+            filename = f"{int(time.time())}.mp3"
+            local_path = f"/tmp/{filename}"
+            remote_path = f"{self.music_dir}/{filename}"
+
+            import yt_dlp
+            ydl_opts = {'format': 'bestaudio/best', 'outtmpl': local_path, 'quiet': True,
+                        'postprocessors': [{'key': 'FFmpegExtractAudio', 'preferredcodec': 'mp3'}]}
+            with yt_dlp.YoutubeDL(ydl_opts) as ydl:
+                ydl.download([url])
+
+            file_manager = self.session.service("ALFileManager")
+            file_manager.put(local_path, remote_path)
+
+            self.current_file = remote_path
+            self.player.playFile(remote_path)
+            self.is_playing = True
+            print(f"♪ Playing on NAO: {title}")
+        except Exception as e:
+            print(f"Playback error: {e}")
+
+    def pause(self):
+        if self.is_playing:
+            self.player.pause()
+            self.is_playing = False
+            print("⏸ Paused")
+        else:
+            self.player.play(self.current_file)
+            self.is_playing = True
+            print("▶ Resumed")
+
+    def stop(self):
+        self.player.stopAll()
+        self.is_playing = False
+        print("⏹ Stopped")
+
+    def seek(self, seconds):
+        try:
+            pos = self.player.getCurrentPosition() / 1000.0
+            self.player.goTo(pos + seconds)
+            print(f"Seeked {seconds:+.0f}s")
+        except:
+            print("Seek unavailable")
+
+
+# ==========================================
+# MAIN TELEOP
 # ==========================================
 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")
         self.tts = session.service("ALTextToSpeech")
-        
         try:
             self.battery = session.service("ALBattery")
         except:
             self.battery = None
 
-        self.video = None
+        self.music = NaoMusicPlayer(session)   # Music on NAO
-        self.video_client = None
 
         self.audio_device = None
         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...")
+        pygame.display.set_caption("NAO Teleop + Music on Robot")
         self.font = pygame.font.SysFont(None, 24)
         self.clock = pygame.time.Clock()
         self.update_title()
@@ -209,7 +261,7 @@ class NaoTeleop:
             self.volume = self.audio_device.getOutputVolume()
             print(f"🔊 NAO speaker volume: {self.volume}%")
         except Exception as e:
-            print(f"⚠️ Could not reach ALAudioDevice: {e}")
+            print(f"⚠️ ALAudioDevice error: {e}")
             self.audio_device = None
 
     def _init_video_maxfps(self):
@@ -219,29 +271,22 @@ class NaoTeleop:
             print("✅ Camera ready")
         except:
             print("❌ Camera not available")
+            self.video_client = None
 
     def _init_mic_stream(self):
-        if not self.audio_device:
+        if not self.audio_device: return
-            print("❌ Can't start mic listening - ALAudioDevice unavailable")
-            return
         try:
-            self.sound_receiver = SoundReceiver(
+            self.sound_receiver = SoundReceiver(self.audio_device_index, 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)
             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"✅ Mic stream ready (gain={self.mic_gain}x)")
-            print(f"✅ Live Audio Stream ready (mic gain = {self.mic_gain}x)")
         except Exception as e:
-            print(f"❌ Audio init failed: {e}")
+            print(f"❌ Mic init failed: {e}")
 
     def get_image(self):
-        if not self.video or not self.video_client:
+        if not hasattr(self, 'video') or not self.video_client:
             return None
         try:
             image = self.video.getImageRemote(self.video_client)
@@ -260,19 +305,12 @@ class NaoTeleop:
         try:
             self.motion.setStiffnesses("RArm", 1.0)
             names = ["RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"]
-            angles = [
+            angles = [[-1.5]*5, [-0.4,-0.7,-0.4,-0.7,-0.4], [0.6,1.1,0.6,1.1,0.6],
-                [-1.5, -1.5, -1.5, -1.5, -1.5],  
+                      [0.8,0.3,0.8,0.3,0.8], [0.0,1.5,0.0,-1.5,0.0], [1.0]*5]
-                [-0.4, -0.7, -0.4, -0.7, -0.4],  
+            times = [[0.5,1.0,1.5,2.0,2.5]] * 6
-                [ 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)
-            
+            neutral = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
-            neutral_angles = [1.5, -0.15, 1.2, 0.5, 0.0, 0.0]
+            self.motion.angleInterpolationWithSpeed(names, neutral, 0.3)
-            self.motion.angleInterpolationWithSpeed(names, neutral_angles, 0.3)
             self.motion.setStiffnesses("RArm", 0.6)
             print("✅ Wave completed")
         except Exception as e:
@@ -283,27 +321,30 @@ class NaoTeleop:
         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_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_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_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_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 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}%")
+        pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol: {self.volume}%")
 
     def change_volume(self, delta):
         self.volume = int(min(100, max(0, self.volume + delta)))
         if self.audio_device:
             try:
                 self.audio_device.setOutputVolume(self.volume)
-            except Exception as e:
+            except: pass
-                print(f"⚠️ Failed to set NAO speaker volume: {e}")
+        print(f"🔊 NAO volume: {self.volume}%")
-        print(f"🔊 NAO speaker volume: {self.volume}%")
         self.update_title()
 
     def check_battery(self):
@@ -312,32 +353,29 @@ class NaoTeleop:
         if now - self.last_batt_check > 10:
             try:
                 self.battery_level = self.battery.getBatteryCharge()
-            except:
+            except: pass
-                pass
             self.update_title()
             self.last_batt_check = now
 
     def run(self):
         self.motion.wakeUp()
         self.posture.goToPosture("StandInit", 0.5)
-        
         time.sleep(1.0)
         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("   WASD / Arrows = Walk")
+        print("   WASD/Arrows = Walk")
-        print("   I J K L       = Head")
+        print("   I J K L     = Head")
-        print("   1             = Wave")
+        print("   SPACE       = Reset head")
-        print("   T             = Type to Speak (TTS)")
+        print("   1           = Wave")
-        print("   -  /  =       = NAO speaker volume down / up")
+        print("   M           = Music search (plays on NAO)")
-        print("   0             = Mute NAO speaker")
+        print("   P           = Pause/Resume music")
-        print("   ESC = Quit")
+        print("   S           = Stop music")
+        print("   ← →         = Seek ±10s")
+        print("   T           = TTS")
+        print("   ESC         = Quit")
 
         while self.running:
             self.check_battery()
@@ -349,15 +387,12 @@ class NaoTeleop:
                     if self.typing_mode:
                         if event.key == pygame.K_RETURN:
                             if self.chat_message.strip():
-                                print(f"🗣️ NAO saying: {self.chat_message}")
                                 threading.Thread(target=self.tts.say, args=(self.chat_message,)).start()
                             self.chat_message = ""
                             self.typing_mode = False
-                            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:
@@ -367,68 +402,69 @@ 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_m:
+                            query = input("Search music to play on NAO: ").strip()
+                            if query:
+                                self.music.search_and_play(query)
+                        elif event.key == pygame.K_p:
+                            self.music.pause()
+                        elif event.key == pygame.K_s:
+                            self.music.stop()
+                        elif event.key == pygame.K_LEFT:
+                            self.music.seek(-10)
+                        elif event.key == pygame.K_RIGHT:
+                            self.music.seek(10)
                         elif event.key == pygame.K_t:
                             self.typing_mode = True
                             self.motion.stopMove()
-                            self.last_batt_check = 0
-                        elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS):
-                            self.change_volume(-5)
-                        elif event.key in (pygame.K_EQUALS, pygame.K_PLUS, pygame.K_KP_PLUS):
-                            self.change_volume(5)
-                        elif event.key == pygame.K_0:
-                            self.change_volume(-self.volume)
 
             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_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_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_a] or keys[pygame.K_LEFT]: theta = 0.5
                 if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.5
 
                 if abs(x) > 0.1 or abs(theta) > 0.1:
                     self.motion.moveToward(x, y, theta, carpet_config)
                 else:
                     self.motion.stopMove()
-
                 self.handle_head_movement()
 
             frame = self.get_image()
             if frame is not None:
-                rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+                up = cv2.resize(frame, (self.display_width, self.display_height), interpolation=cv2.INTER_CUBIC)
+                rgb = cv2.cvtColor(up, 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))
+                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()
             self.clock.tick(0)
 
         print("🛑 Shutting down...")
         self.motion.stopMove()
-        
+        self.music.stop()
         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):
             self.sound_receiver.close()
-
+        if hasattr(self, 'video') and self.video_client:
-        if self.video and self.video_client:
+            try: self.video.unsubscribe(self.video_client)
-            try:
+            except: pass
-                self.video.unsubscribe(self.video_client)
-            except:
-                pass
-                
         self.motion.rest()
         pygame.quit()
 
@@ -441,8 +477,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,
+    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()
 
     session = qi.Session()
@@ -453,6 +489,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()

readme.md

@@ -0,0 +1,24 @@
+# NAO Walk - Teleoperation + Music Player
+
+A full-featured Python teleoperation tool for the NAO robot with live camera feed, head control, microphone streaming to your laptop, and **music playback directly on the robot**.
+
+---
+
+## Features
+
+- **Live Camera Feed** from NAO (upscaled on PC)
+- **Keyboard Teleop** (WASD / Arrow keys)
+- **Head Control** (I J K L)
+- **Wave Gesture** (Key `1`)
+- **Live Mic Streaming** from NAO to your laptop (with adjustable gain)
+- **Music Player on NAO** — Search & play YouTube music directly through the robot's speakers
+- **TTS Chat** — Type messages for the robot to speak (`T`)
+- **Battery & Volume Display**
+
+---
+
+## Requirements
+
+### On your laptop:
+```bash
+pip install qi opencv-python numpy pygame yt-dlp