diff --git a/naowalk.py b/naowalk.py index e07a8d8..a2f4037 100644 --- a/naowalk.py +++ b/naowalk.py @@ -10,143 +10,117 @@ import cv2 import numpy as np class NaoTeleop: - def __init__(self, session, robot_ip="127.0.0.1", port=9559): + def __init__(self, session): + self.session = session self.motion = session.service("ALMotion") self.posture = session.service("ALRobotPosture") - self.video = session.service("ALVideoDevice") - - self.robot_ip = robot_ip - self.running = True - - # Video settings (using numbers instead of vision_definitions) - self.resolution = 2 # 2 = kQVGA (320x240) - self.color_space = 11 # 11 = kRGBColorSpace - self.fps = 15 - + self.video = None self.video_client = None - try: - self.video_client = self.video.subscribe("TeleopCamera", self.resolution, self.color_space, self.fps) - print("āœ… Camera subscribed successfully") - except Exception as e: - print(f"āš ļø Camera subscription failed: {e}") - - # Pygame setup + self.running = True + + print("šŸ” Initializing video...") + self._init_video() + pygame.init() self.screen = pygame.display.set_mode((640, 480)) - pygame.display.set_caption("NAO Teleoperation - Camera + Controls") + pygame.display.set_caption("NAO Teleoperation") self.clock = pygame.time.Clock() - - # Movement - self.speed = 0.8 - self.turn_speed = 0.6 - self.strafe = 0.5 + + def _init_video(self): + try: + self.video = self.session.service("ALVideoDevice") + print("āœ… ALVideoDevice service found") + except Exception as e: + print(f"āŒ Video service error: {e}") + 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 + 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}") def get_image(self): - if not self.video_client: + if not self.video or not self.video_client: return None try: image = self.video.getImageRemote(self.video_client) - if not image: - return None - - width = image[0] - height = image[1] - array = image[6] - - image_array = np.frombuffer(bytearray(array), dtype=np.uint8).reshape((height, width, 3)) - return cv2.cvtColor(image_array, cv2.COLOR_RGB2BGR) + 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) except: - return None - - def handle_input(self): - keys = pygame.key.get_pressed() - x = y = theta = 0.0 - - # Keyboard - if keys[pygame.K_w] or keys[pygame.K_UP]: x = self.speed - if keys[pygame.K_s] or keys[pygame.K_DOWN]: x = -self.speed - if keys[pygame.K_a] or keys[pygame.K_LEFT]: theta = self.turn_speed - if keys[pygame.K_d] or keys[pygame.K_RIGHT]: theta = -self.turn_speed - if keys[pygame.K_q]: y = self.strafe - if keys[pygame.K_e]: y = -self.strafe - - # Game Controller - if pygame.joystick.get_count() > 0: - joy = pygame.joystick.Joystick(0) - joy.init() - - x = -joy.get_axis(1) * self.speed # Left stick Y - y = joy.get_axis(0) * self.strafe # Left stick X - theta = -joy.get_axis(3) * self.turn_speed # Right stick X (turning) - - if joy.get_button(0): # A / X button - self.posture.goToPosture("StandInit", 0.5) - - # Send command - if abs(x) > 0.1 or abs(y) > 0.1 or abs(theta) > 0.1: - self.motion.moveToward(x, y, theta, [["Frequency", 1.0]]) - else: - self.motion.stopMove() + pass + return None def run(self): self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) - self.motion.setMoveArmsEnabled(True, True) - - print("\nšŸŽ® NAO Teleop Started!") - print("WASD / Arrows = Move & Turn") - print("Q/E = Strafe left/right") - print("ESC = Quit") - - try: - 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 - - self.handle_input() - - frame = self.get_image() - if frame is not None: - frame = cv2.resize(frame, (640, 480)) - frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - surf = pygame.surfarray.make_surface(frame_rgb.swapaxes(0, 1)) - self.screen.blit(surf, (0, 0)) - else: - # Show placeholder if no camera - self.screen.fill((30, 30, 30)) - font = pygame.font.SysFont(None, 48) - text = font.render("Waiting for NAO Camera...", True, (255, 100, 100)) - self.screen.blit(text, (100, 200)) - - pygame.display.flip() - self.clock.tick(30) - - finally: - print("šŸ›‘ Shutting down...") - self.motion.stopMove() - self.motion.rest() - if self.video_client: - self.video.unsubscribe(self.video_client) - pygame.quit() + print("šŸŽ® Controls Active - WASD/Arrows | ESC to quit") -def main(): + 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 + + 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 abs(x) > 0.1 or abs(theta) > 0.1: + self.motion.moveToward(x, y, theta) + else: + self.motion.stopMove() + + 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)) + + pygame.display.flip() + self.clock.tick(25) + + # 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", help="Robot IP address") - parser.add_argument("--port", type=int, default=9559, help="Naoqi port") - + 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 NAO at {args.ip}:{args.port}") - except RuntimeError: - print(f"āŒ Can't connect to Naoqi at {args.ip}:{args.port}") + print(f"āœ… Connected to {args.ip}") + except Exception as e: + print(f"Connection failed: {e}") sys.exit(1) - - teleop = NaoTeleop(session, args.ip, args.port) - teleop.run() -if __name__ == "__main__": - main() + NaoTeleop(session).run()