Lucca Pirovano
132 lines
4.3 KiB
Python
132 lines
4.3 KiB
Python
#!/usr/bin/env python
|
|
# -*- encoding: UTF-8 -*-
|
|
|
|
import qi
|
|
import argparse
|
|
import sys
|
|
import time
|
|
import pygame
|
|
import cv2
|
|
import numpy as np
|
|
|
|
class NaoTeleop:
|
|
def __init__(self, session):
|
|
self.session = session
|
|
self.motion = session.service("ALMotion")
|
|
self.posture = session.service("ALRobotPosture")
|
|
self.video = None
|
|
self.video_client = None
|
|
self.running = True
|
|
|
|
# Ultra-optimized video setup
|
|
self._init_video_maxfps()
|
|
|
|
pygame.init()
|
|
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_maxfps(self):
|
|
try:
|
|
self.video = self.session.service("ALVideoDevice")
|
|
print("✅ ALVideoDevice found")
|
|
except:
|
|
print("❌ No video service")
|
|
return
|
|
|
|
try:
|
|
# 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"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:
|
|
return None
|
|
try:
|
|
image = self.video.getImageRemote(self.video_client)
|
|
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))
|
|
frame = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
|
|
# Minimal processing
|
|
return cv2.resize(frame, (320, 240), interpolation=cv2.INTER_NEAREST)
|
|
except:
|
|
pass
|
|
return None
|
|
|
|
def run(self):
|
|
self.motion.wakeUp()
|
|
self.posture.goToPosture("StandInit", 0.5)
|
|
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.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:
|
|
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((10, 10, 30))
|
|
|
|
pygame.display.flip()
|
|
self.clock.tick(0) # MAX speed (no limit)
|
|
|
|
# Clean shutdown
|
|
print("Shutting down...")
|
|
self.motion.stopMove()
|
|
self.motion.rest()
|
|
if self.video and self.video_client:
|
|
try:
|
|
self.video.unsubscribe(self.video_client)
|
|
except:
|
|
pass
|
|
pygame.quit()
|
|
|
|
if __name__ == "__main__":
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("--ip", type=str, default="127.0.0.1")
|
|
parser.add_argument("--port", type=int, default=9559)
|
|
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).run()
|