63 lines
1.8 KiB
Python
63 lines
1.8 KiB
Python
|
import socket
|
||
|
|
import pygame
|
||
|
|
from argparse import ArgumentParser
|
||
|
|
|
||
|
|
class ManualController:
|
||
|
|
def __init__(self, estimator):
|
||
|
|
self.estimator = estimator
|
||
|
|
self.estimator.external_keyboard_callback = self.handle_keypress
|
||
|
|
|
||
|
|
self.controlling = False
|
||
|
|
|
||
|
|
def handle_keypress(self, key):
|
||
|
|
pass
|
||
|
|
|
||
|
|
parser = ArgumentParser()
|
||
|
|
parser.add_argument('ip', metavar='ip', type=str, help='ip address of the controlled robot')
|
||
|
|
args = parser.parse_args()
|
||
|
|
|
||
|
|
ip = args.ip
|
||
|
|
|
||
|
|
pygame.init()
|
||
|
|
pygame.display.set_mode((640, 480))
|
||
|
|
|
||
|
|
rc_socket = socket.socket()
|
||
|
|
try:
|
||
|
|
rc_socket.connect((ip, 1234)) # connect to robot
|
||
|
|
except socket.error:
|
||
|
|
print("could not connect to socket")
|
||
|
|
|
||
|
|
running = True
|
||
|
|
while running:
|
||
|
|
u1 = 0
|
||
|
|
u2 = 0
|
||
|
|
vmax = 1.0
|
||
|
|
events = pygame.event.get()
|
||
|
|
for event in events:
|
||
|
|
if event.type == pygame.KEYDOWN:
|
||
|
|
if event.key == pygame.K_LEFT:
|
||
|
|
u1 = -vmax
|
||
|
|
u2 = vmax
|
||
|
|
print(f"turn left: ({u1},{u2})")
|
||
|
|
elif event.key == pygame.K_RIGHT:
|
||
|
|
u1 = vmax
|
||
|
|
u2 = -vmax
|
||
|
|
print(f"turn right: ({u1},{u2})")
|
||
|
|
elif event.key == pygame.K_UP:
|
||
|
|
u1 = vmax
|
||
|
|
u2 = vmax
|
||
|
|
print(f"forward: ({u1},{u2})")
|
||
|
|
elif event.key == pygame.K_DOWN:
|
||
|
|
u1 = -vmax
|
||
|
|
u2 = -vmax
|
||
|
|
print(f"backward: ({u1},{u2})")
|
||
|
|
elif event.key == pygame.K_ESCAPE:
|
||
|
|
print("quit")
|
||
|
|
running = False
|
||
|
|
u1 = 0.0
|
||
|
|
u2 = 0.0
|
||
|
|
rc_socket.send(f'({u1},{u2})\n'.encode())
|
||
|
|
elif event.type == pygame.KEYUP:
|
||
|
|
print("key released, resetting: ({},{})".format(u1, u2))
|
||
|
|
rc_socket.send(f'({u1},{u2})\n'.encode())
|