diff --git a/__pycache__/naomusic.cpython-311.pyc b/__pycache__/naomusic.cpython-311.pyc new file mode 100644 index 0000000..80091de Binary files /dev/null and b/__pycache__/naomusic.cpython-311.pyc differ diff --git a/naowalk.py b/naowalk.py index 6b8b0f0..5daae3a 100644 --- a/naowalk.py +++ b/naowalk.py @@ -270,4 +270,269 @@ class NaoTeleop: def update_title(self): mode = "[TYPING] " if self.typing_mode else "" - pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol + pygame.display.set_caption(f"NAO Teleop | {mode}Battery: {self.battery_level}% | Vol: {self.volume}% | Speed: {self.walk_speed:.2f}") + + def change_volume(self, delta): + self.volume = int(min(100, max(0, self.volume + delta))) + if self.audio_device: + self._safe("Volume set", lambda: self.audio_device.setOutputVolume(self.volume)) + print(f"šŸ”Š NAO volume: {self.volume}%") + self.update_title() + + def check_battery(self): + if not self.battery: return + now = time.time() + if now - self.last_batt_check > 10: + try: + self.battery_level = self.battery.getBatteryCharge() + except: 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) + + print("šŸ¦¶ Initializing walk engine...") + self._safe("moveInit", self.motion.moveInit) + print("šŸ¦¶ Walk engine ready") + + print("\nšŸŽ® Controls: WASD/Arrows=Walk | [/]=Speed | IJKL=Head | SPACE=Reset head | 1=Wave | M=Music search (ā†‘ā†“ select, Enter confirm) | X=Stop | -/==Volume | T=TTS | ESC=Quit") + + try: + while self.running: + try: + self.check_battery() + + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self.running = False + elif event.type == pygame.KEYDOWN: + if self.music_search_mode: + if event.key == pygame.K_RETURN: + if self.music_results: + entry = self.music_results[self.selected_result] + threading.Thread( + target=self.music.play, + args=(entry['url'], entry.get('title', 'Unknown')), + daemon=True + ).start() + self.music_search_mode = False + elif self.music_search_text.strip() and not self.music_search_pending: + query = self.music_search_text + self.music_search_pending = True + def do_search(q=query): + results = self.music.search(q) + self.music_results = results + self.selected_result = 0 + self.music_search_pending = False + if not results: + self.music_search_mode = False + threading.Thread(target=do_search, daemon=True).start() + else: + self.music_search_mode = False + elif event.key == pygame.K_ESCAPE: + self.music_search_mode = False + self.music_search_text = "" + self.music_results = [] + elif event.key == pygame.K_UP: + if self.music_results: + self.selected_result = (self.selected_result - 1) % len(self.music_results[:5]) + elif event.key == pygame.K_DOWN: + if self.music_results: + self.selected_result = (self.selected_result + 1) % len(self.music_results[:5]) + elif event.key == pygame.K_BACKSPACE: + self.music_search_text = self.music_search_text[:-1] + self.music_results = [] + elif event.key in (pygame.K_1, pygame.K_2, pygame.K_3, pygame.K_4, pygame.K_5): + idx = int(event.unicode) - 1 + if idx < len(self.music_results): + entry = self.music_results[idx] + threading.Thread( + target=self.music.play, + args=(entry['url'], entry.get('title', 'Unknown')), + daemon=True + ).start() + self.music_search_mode = False + elif event.unicode.isprintable(): + self.music_search_text += event.unicode + self.music_results = [] + elif self.typing_mode: + if event.key == pygame.K_RETURN: + if self.chat_message.strip(): + threading.Thread(target=self.tts.say, args=(self.chat_message,), daemon=True).start() + self.chat_message = "" + self.typing_mode = False + elif event.key == pygame.K_ESCAPE: + self.chat_message = "" + self.typing_mode = False + elif event.key == pygame.K_BACKSPACE: + self.chat_message = self.chat_message[:-1] + else: + self.chat_message += event.unicode + else: + if event.key == pygame.K_ESCAPE: + 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: + self.music_search_mode = True + self.music_search_text = "" + self.music_results = [] + elif event.key == pygame.K_x: + self.music.stop() + elif event.key in (pygame.K_MINUS, pygame.K_KP_MINUS): + self.change_volume(-5) + elif event.key in (pygame.K_EQUALS, pygame.K_KP_PLUS): + self.change_volume(5) + elif event.key == pygame.K_t: + self.typing_mode = True + self._safe("stopMove", self.motion.stopMove) + + if not self.typing_mode and not self.music_search_mode: + keys = pygame.key.get_pressed() + + speed_changed = False + if keys[pygame.K_LEFTBRACKET]: + self.walk_speed = max(0.1, self.walk_speed - 0.01) + speed_changed = True + if keys[pygame.K_RIGHTBRACKET]: + self.walk_speed = min(1.0, self.walk_speed + 0.01) + speed_changed = True + if speed_changed: + self.update_title() + + x = y = theta = 0.0 + if keys[pygame.K_w] or keys[pygame.K_UP]: x = self.walk_speed + if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -self.walk_speed + if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = self.walk_speed * 0.8 + if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -self.walk_speed * 0.8 + + try: + if abs(x) > 0.05 or abs(theta) > 0.05: + stable_config = [ + ["StepHeight", 0.02], + ["MaxStepFrequency", 0.5], + ["TorsoWy", 0.08], + ["MaxStepX", 0.03] + ] + self.motion.moveToward(x, y, theta, stable_config) + else: + self.motion.stopMove() + except Exception as e: + print(f"Move error: {e}") + self.handle_head_movement() + + frame = self.get_image() + if frame is not None: + 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)) + + # Music status + if self.music.is_loading: + text = self.font.render(f"ā™Ŗ {self.music.status}", True, (255, 200, 0)) + self.screen.blit(text, (10, 10)) + if self.music.progress > 0: + bar_w = 220 + pct = min(self.music.progress, 100) / 100.0 + pygame.draw.rect(self.screen, (70, 70, 70), (10, 32, bar_w, 8)) + pygame.draw.rect(self.screen, (255, 200, 0), (10, 32, int(bar_w * pct), 8)) + elif self.music.load_error: + text = self.font.render(f"ā™Ŗ Error: {self.music.load_error}", True, (255, 80, 80)) + self.screen.blit(text, (10, 10)) + elif self.music.current_title: + text = self.font.render(f"ā™Ŗ {self.music.current_title[:45]}", True, (0, 255, 100)) + self.screen.blit(text, (10, 10)) + + # Music search interface + if self.music_search_mode: + box_h = min(self.display_height - 80, 260) + s = pygame.Surface((self.display_width, box_h)) + s.set_alpha(220) + s.fill((0, 0, 40)) + self.screen.blit(s, (0, 80)) + + text = self.font.render(f"Search: {self.music_search_text}", True, (255, 255, 255)) + self.screen.blit(text, (20, 100)) + + if self.music_results: + shown = self.music_results[:5] + for i, entry in enumerate(shown): + row_y = 140 + i * 30 + if i == self.selected_result: + pygame.draw.rect(self.screen, (90, 90, 0), (16, row_y - 3, self.display_width - 32, 26)) + color = (255, 255, 0) if i == self.selected_result else (200, 200, 200) + title = entry.get('title', 'No title')[:60] + line = self.font.render(f"{i+1}. {title}", True, color) + self.screen.blit(line, (20, row_y)) + hint_y = 140 + len(shown) * 30 + 6 + hint = self.font.render("\u2191\u2193 Select Enter Confirm 1-5 Quick pick Esc Cancel", True, (150, 150, 150)) + self.screen.blit(hint, (20, hint_y)) + elif self.music_search_pending: + text = self.font.render("Searching...", True, (200, 200, 200)) + self.screen.blit(text, (20, 140)) + else: + text = self.font.render("No results. Type to search again, Enter to search", True, (200, 200, 200)) + self.screen.blit(text, (20, 140)) + + # TTS box + if self.typing_mode: + s = pygame.Surface((self.display_width, 40)) + s.set_alpha(180) + s.fill((0,0,0)) + 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) + except Exception as e: + print(f"\u26a0\ufe0f Frame error (continuing): {e}") + finally: + print("šŸ›‘ Shutting down...") + self._safe("stopMove", self.motion.stopMove) + self._safe("music stop", self.music.stop) + if self.audio_device: + self._safe("audio unsubscribe", lambda: self.audio_device.unsubscribe(self.audio_service_name)) + if getattr(self, "sound_receiver", None): + self._safe("sound receiver close", self.sound_receiver.close) + if hasattr(self, 'video') and self.video_client: + self._safe("video unsubscribe", lambda: self.video.unsubscribe(self.video_client)) + self._safe("rest", self.motion.rest) + pygame.quit() + + +if __name__ == "__main__": + MIC_CHANNELS = {"left": 1, "right": 2, "front": 3, "rear": 4} + + parser = argparse.ArgumentParser() + parser.add_argument("--ip", type=str, default="127.0.0.1") + 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("--video-scale", type=float, default=2.0) + args = parser.parse_args() + + session = qi.Session() + try: + session.connect(f"tcp://{args.ip}:{args.port}") + print(f"āœ… Connected to {args.ip}") + except Exception as e: + print(f"āŒ Connection failed: {e}") + sys.exit(1) + + NaoTeleop(session, nao_ip=args.ip, + audio_device_index=args.audio_device_index, + mic_channel=MIC_CHANNELS[args.mic_channel], + mic_gain=args.mic_gain, + video_scale=args.video_scale).run() +