made framerate way better

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)