added more scripts, walking works great but no camera yet
This commit is contained in:
Lucca Pirovano
@@ -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()
|
||||
Executable
+1
@@ -0,0 +1 @@
|
||||
python3 disable_autonomous_life.py --ip spike.local --port 9561
|
||||
+51
-58
@@ -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 = 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"""
|
||||
if not self.video_client:
|
||||
return None
|
||||
try:
|
||||
image = self.video.getImageRemote(self.video_client)
|
||||
if image is None:
|
||||
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) # OpenCV uses BGR
|
||||
return cv2.cvtColor(image_array, cv2.COLOR_RGB2BGR)
|
||||
except:
|
||||
return None
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
# 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
|
||||
if joy.get_button(0): # A / X button
|
||||
self.posture.goToPosture("StandInit", 0.5)
|
||||
if joy.get_button(1): # B/O button - rest
|
||||
self.motion.rest()
|
||||
|
||||
# Apply movement (non-blocking)
|
||||
# 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)
|
||||
|
||||
Reference in New Issue
Block a user