From 92e7395cf124c23f87f24edc14aba2c2c7236411 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 13:39:07 -0400 Subject: [PATCH] made framerate way better --- naowalk.py | 71 +++++++++++++++++++++++++++++------------------------- 1 file changed, 38 insertions(+), 33 deletions(-) diff --git a/naowalk.py b/naowalk.py index a2f4037..80186e0 100644 --- a/naowalk.py +++ b/naowalk.py @@ -18,36 +18,40 @@ class NaoTeleop: self.video_client = None self.running = True - print("🔍 Initializing video...") - self._init_video() + # Ultra-optimized video setup + self._init_video_maxfps() pygame.init() - self.screen = pygame.display.set_mode((640, 480)) - pygame.display.set_caption("NAO Teleoperation") + self.screen = pygame.display.set_mode((320, 240)) # Smaller window = faster + pygame.display.set_caption("NAO Teleop - MAX FPS") self.clock = pygame.time.Clock() - def _init_video(self): + def _init_video_maxfps(self): try: self.video = self.session.service("ALVideoDevice") - print("✅ ALVideoDevice service found") - except Exception as e: - print(f"❌ Video service error: {e}") + print("✅ ALVideoDevice found") + except: + print("❌ No video service") return - # Try subscribeCamera (newer method) try: - self.video_client = self.video.subscribeCamera("TeleopCam", 0, 2, 11, 10) # camera 0 = top - print("✅ Camera subscribed using subscribeCamera!") - return + # MAX SPEED SETTINGS: + self.video_client = self.video.subscribeCamera( + "MaxFpsCam", + 0, # Top camera + 0, # 0 = kQQVGA (80x60) → fastest + 11, # RGB + 30 # Ask for 30fps (realistic ~15-20) + ) + print("🚀 Camera subscribed in MAX FPS mode (80x60)") except Exception as e: - print(f"subscribeCamera failed: {e}") - - # Fallback to old subscribe - try: - self.video_client = self.video.subscribe("TeleopCam", 2, 11, 10) - print("✅ Camera subscribed using old method") - except Exception as e: - print(f"❌ All subscription methods failed: {e}") + print(f"Subscription failed: {e}") + # Fallback + try: + self.video_client = self.video.subscribe("MaxFpsCam", 0, 11, 20) + print("✅ Fallback subscription used") + except: + pass def get_image(self): if not self.video or not self.video_client: @@ -57,7 +61,9 @@ class NaoTeleop: if image and len(image) >= 7: w, h = image[0], image[1] arr = np.frombuffer(bytearray(image[6]), dtype=np.uint8).reshape((h, w, 3)) - return cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) + frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) + # Minimal processing + return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST) except: pass return None @@ -65,41 +71,40 @@ class NaoTeleop: def run(self): self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) - print("🎮 Controls Active - WASD/Arrows | ESC to quit") + print("🚀 MAX FPS MODE ACTIVE - WASD/Arrows | ESC to quit") while self.running: for event in pygame.event.get(): if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): self.running = False + # Fast input handling keys = pygame.key.get_pressed() x = y = theta = 0.0 - if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.8 - if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.8 - if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.6 - if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.6 + if keys[pygame.K_w] or keys[pygame.K_UP]: x = 0.9 + if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -0.9 + if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = 0.7 + if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -0.7 if abs(x) > 0.1 or abs(theta) > 0.1: self.motion.moveToward(x, y, theta) else: self.motion.stopMove() + # Camera frame = self.get_image() if frame is not None: - frame = cv2.resize(frame, (640, 480)) rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) surf = pygame.surfarray.make_surface(rgb.swapaxes(0,1)) self.screen.blit(surf, (0, 0)) else: - self.screen.fill((30, 30, 50)) - font = pygame.font.SysFont(None, 38) - self.screen.blit(font.render("Camera Feed Not Available", True, (255, 100, 100)), (100, 200)) + self.screen.fill((10, 10, 30)) pygame.display.flip() - self.clock.tick(25) + self.clock.tick(0) # MAX speed (no limit) # Clean shutdown - print("🛑 Shutting down...") + print("Shutting down...") self.motion.stopMove() self.motion.rest() if self.video and self.video_client: @@ -118,7 +123,7 @@ if __name__ == "__main__": session = qi.Session() try: session.connect(f"tcp://{args.ip}:{args.port}") - print(f"✅ Connected to {args.ip}") + print(f"Connected to {args.ip}") except Exception as e: print(f"Connection failed: {e}") sys.exit(1)