From 228fa591d87ea4eb38572c2cb2ea5767956b27e9 Mon Sep 17 00:00:00 2001 From: Lucca Pirovano Date: Wed, 15 Jul 2026 13:31:06 -0400 Subject: [PATCH] added more scripts, walking works great but no camera yet --- disable_autonomous_life.py | 46 ++++++++++++++ disable_autonomous_life.sh | 1 + naowalk.py | 127 ++++++++++++++++++------------------- 3 files changed, 107 insertions(+), 67 deletions(-) create mode 100644 disable_autonomous_life.py create mode 100755 disable_autonomous_life.sh diff --git a/disable_autonomous_life.py b/disable_autonomous_life.py new file mode 100644 index 0000000..9e9f5f8 --- /dev/null +++ b/disable_autonomous_life.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +# -*- encoding: UTF-8 -*- + +import qi +import argparse +import sys + +def 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 number") + + args = parser.parse_args() + + session = qi.Session() + try: + session.connect(f"tcp://{args.ip}:{args.port}") + print(f"Connected to NAO at {args.ip}") + except RuntimeError: + print(f"❌ Could not connect to Naoqi at {args.ip}:{args.port}") + sys.exit(1) + + # Get the ALAutonomousLife service + try: + autonomous_life = session.service("ALAutonomousLife") + + current_state = autonomous_life.getState() + print(f"Current Autonomous Life state: {current_state}") + + if current_state != "disabled": + print("Disabling Autonomous Life...") + autonomous_life.setState("disabled") + print("✅ Autonomous Life is now DISABLED") + else: + print("✅ Autonomous Life was already disabled") + + except Exception as e: + print(f"⚠️ Error: {e}") + print("Note: Make sure the robot is awake and you have the right permissions.") + + print("\nYou can now run your teleoperation program safely.") + +if __name__ == "__main__": + main() diff --git a/disable_autonomous_life.sh b/disable_autonomous_life.sh new file mode 100755 index 0000000..46483b1 --- /dev/null +++ b/disable_autonomous_life.sh @@ -0,0 +1 @@ +python3 disable_autonomous_life.py --ip spike.local --port 9561 diff --git a/naowalk.py b/naowalk.py index 9f16507..e07a8d8 100644 --- a/naowalk.py +++ b/naowalk.py @@ -5,13 +5,9 @@ import qi import argparse import sys import time -import threading +import pygame import cv2 import numpy as np -import pygame -from pygame.locals import * - -import vision_definitions class NaoTeleop: def __init__(self, session, robot_ip="127.0.0.1", port=9559): @@ -22,11 +18,17 @@ class NaoTeleop: self.robot_ip = robot_ip self.running = True - # Video client - self.resolution = vision_definitions.kQVGA # 320x240 - self.color_space = vision_definitions.kRGBColorSpace + # 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_client = self.video.subscribe("TeleopCamera", self.resolution, self.color_space, self.fps) + + 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 pygame.init() @@ -34,102 +36,94 @@ class NaoTeleop: pygame.display.set_caption("NAO Teleoperation - Camera + Controls") self.clock = pygame.time.Clock() - # Movement params - self.speed = 0.8 # forward/back + # Movement + self.speed = 0.8 self.turn_speed = 0.6 self.strafe = 0.5 def get_image(self): - """Get latest image from NAO camera""" - image = self.video.getImageRemote(self.video_client) - if image is None: + if 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) + except: 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) # OpenCV uses BGR def handle_input(self): - """Handle keyboard + gamepad input""" keys = pygame.key.get_pressed() x = y = theta = 0.0 - # Keyboard (WASD + arrows) - if keys[K_w] or keys[K_UP]: - x = self.speed - if keys[K_s] or keys[K_DOWN]: - x = -self.speed - if keys[K_a] or keys[K_LEFT]: - theta = self.turn_speed - if keys[K_d] or keys[K_RIGHT]: - theta = -self.turn_speed - if keys[K_q]: - y = self.strafe # strafe left - if keys[K_e]: - y = -self.strafe # strafe right + # 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 (Xbox/PlayStation style) + # Game Controller if pygame.joystick.get_count() > 0: joy = pygame.joystick.Joystick(0) joy.init() - # Left stick: forward/back + strafe - axis_x = joy.get_axis(0) # strafe - axis_y = joy.get_axis(1) # forward/back (usually inverted) - x = -axis_y * self.speed - y = axis_x * self.strafe - - # Right stick or D-pad for turning - turn_axis = joy.get_axis(3) if joy.get_numbuttons() > 3 else 0 - theta = -turn_axis * self.turn_speed - - # Buttons for speed / posture - if joy.get_button(0): # A/X button - stand - self.posture.goToPosture("StandInit", 0.5) - if joy.get_button(1): # B/O button - rest - self.motion.rest() + 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) - # Apply movement (non-blocking) + 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() def run(self): - # Wake up and stand self.motion.wakeUp() self.posture.goToPosture("StandInit", 0.5) self.motion.setMoveArmsEnabled(True, True) - print("Controls:") - print(" WASD / Arrow keys - Move / Turn") - print(" Q/E - Strafe left/right") - print(" Gamepad: Left stick move, Right stick turn") - print(" Press ESC or close window to quit") + 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 == QUIT or (event.type == KEYDOWN and event.key == K_ESCAPE): + if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE): self.running = False self.handle_input() - # Get and display camera frame = self.get_image() if frame is not None: frame = cv2.resize(frame, (640, 480)) - # Convert to Pygame surface - frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - surf = pygame.surfarray.make_surface(frame.swapaxes(0,1)) + 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: @@ -138,18 +132,17 @@ class NaoTeleop: def 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", help="Robot IP address") + parser.add_argument("--port", type=int, default=9559, help="Naoqi port") 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"❌ Can't connect to Naoqi at {args.ip}:{args.port}") sys.exit(1) teleop = NaoTeleop(session, args.ip, args.port)