Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

Compare commits

base: Telos4:019c4590aa834a92bd2237f7076f5461d54e45f3
Branches Tags
Telos4:master
Telos4:simple_control
..
compare: Telos4:4c3c3f973e17ee9441fb16cca31828e0e84365c4
Branches Tags
Telos4:master
Telos4:simple_control

No commits in common. "019c4590aa834a92bd2237f7076f5461d54e45f3" and "4c3c3f973e17ee9441fb16cca31828e0e84365c4" have entirely different histories.
019c4590aa ... 4c3c3f973e

6 changed files with 55 additions and 144 deletions
Show Stats Expand all files Collapse all files

15
gauss-turing/webserver/event_server_comm.py
View File

@ -1,15 +0,0 @@
import socket
import json
HOST, PORT = "localhost", 42424
def move_grid(x, y, orientation, dimx, dimy):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((HOST, PORT))
payload = json.dumps({"x": x, "y": y, "dimx": dimx, "dimy": dimy, "orientation": orientation})
sock.sendall(f"move_grid_blocking;{payload}\n".encode())
reply = sock.recv(1024)
print(reply)
if __name__ == "__main__":
move_grid(1,1,'^', 5, 5)

58
gauss-turing/webserver/gauss_turing.py
View File

@ -3,8 +3,6 @@ import random
import pygame import pygame
import os import os
from event_server_comm import move_grid
BLACK = np.array([0, 0, 0], dtype=np.uint8) BLACK = np.array([0, 0, 0], dtype=np.uint8)
WHITE = np.array([255, 255, 255], dtype=np.uint8) WHITE = np.array([255, 255, 255], dtype=np.uint8)
RED = np.array([255, 0, 0], dtype=np.uint8) RED = np.array([255, 0, 0], dtype=np.uint8)
@ -24,7 +22,6 @@ P0_text = myfont.render('P0', False, tuple(BLACK))
tiledt = np.dtype([('x', np.uint8), ('y', np.uint8), ('color', np.uint8, 3), ('star', np.bool)]) tiledt = np.dtype([('x', np.uint8), ('y', np.uint8), ('color', np.uint8, 3), ('star', np.bool)])
class Board: class Board:
valid_colors = [WHITE, RED, BLUE] valid_colors = [WHITE, RED, BLUE]
@ -32,6 +29,8 @@ class Board:
self.tiles = np.zeros((dim_y, dim_x), dtype=tiledt) self.tiles = np.zeros((dim_y, dim_x), dtype=tiledt)
for x in range(dim_x): for x in range(dim_x):
for y in range(dim_y): for y in range(dim_y):
self.tiles[y, x]['x'] = x
self.tiles[y, x]['y'] = y
self.tiles[y, x]['color'] = random.choice(Board.valid_colors) self.tiles[y, x]['color'] = random.choice(Board.valid_colors)
def render(self, scale_fac): def render(self, scale_fac):
@ -52,12 +51,6 @@ class Board:
return board_surf return board_surf
def get_xdims(self):
return self.tiles.shape[1]
def get_ydims(self):
return self.tiles.shape[0]
# def __repr__(self): # def __repr__(self):
# s = '' # s = ''
# for y in range(self.tiles.shape[0]): # for y in range(self.tiles.shape[0]):
@ -110,9 +103,6 @@ class Robot:
robot_surf = pygame.transform.rotate(robot_surf, self.get_angle()) robot_surf = pygame.transform.rotate(robot_surf, self.get_angle())
return robot_surf return robot_surf
def update_pos(self, dimx, dimy):
move_grid(self.x, self.y, self.orientation, dimx, dimy)
def __repr__(self): def __repr__(self):
return f"({self.y}, {self.x}) - {self.orientation}" return f"({self.y}, {self.x}) - {self.orientation}"
@ -193,7 +183,7 @@ class Program:
self.prg_counter = 0 self.prg_counter = 0
self.screen_rect = None self.screen_rect = None
def step(self, state='running'): def step(self):
cmd = self.cmds[self.prg_counter] cmd = self.cmds[self.prg_counter]
self.prg_counter += 1 self.prg_counter += 1
@ -211,15 +201,24 @@ class Program:
ynew, xnew = self.robot.get_forward_coordinates() ynew, xnew = self.robot.get_forward_coordinates()
self.robot.x = xnew self.robot.x = xnew
self.robot.y = ynew self.robot.y = ynew
self.robot.update_pos(self.board.get_xdims(), self.board.get_ydims())
elif cmd.action in {'left', 'right'}: elif cmd.action in {'left', 'right'}:
self.robot.orientation = Robot.resulting_orientation[self.robot.orientation][cmd.action] self.robot.orientation = Robot.resulting_orientation[self.robot.orientation][cmd.action]
self.robot.update_pos(self.board.get_xdims(), self.board.get_ydims())
elif cmd.action == 'P0': elif cmd.action == 'P0':
self.prg_counter = 0 self.prg_counter = 0
else: else:
print("color not matching -> skipping command") print("color not matching -> skipping command")
# get all events
ev = pygame.event.get()
# proceed events
for event in ev:
# handle MOUSEBUTTONUP
if event.type == pygame.MOUSEBUTTONUP:
pos = pygame.mouse.get_pos()
if pos[0] >= 325 and pos[0] <= 400 and pos[1] >= 600 and pos[1] <= 650:
print(f"clicked at pos = {pos}")
self.state = 'input'
# update state for new robot position # update state for new robot position
if (not (0 <= self.robot.x < self.board.tiles.shape[1])) or not (0 <= self.robot.y < self.board.tiles.shape[0]): if (not (0 <= self.robot.x < self.board.tiles.shape[1])) or not (0 <= self.robot.y < self.board.tiles.shape[0]):
# robot leaves the board -> GAME OVER # robot leaves the board -> GAME OVER
@ -236,7 +235,7 @@ class Program:
return 'won' return 'won'
# by default we continue in the running state # by default we continue in the running state
return state return 'running'
def render(self, scale_fac, prg_counter_override=None): def render(self, scale_fac, prg_counter_override=None):
"""Render the current program. This will render all commands and highlight the next command to execute """Render the current program. This will render all commands and highlight the next command to execute
@ -278,14 +277,12 @@ class Game:
def __init__(self, dimx, dimy, robotx, roboty): def __init__(self, dimx, dimy, robotx, roboty):
self.robot = Robot(x=robotx, y=roboty, orientation='v') self.robot = Robot(x=robotx, y=roboty, orientation='v')
self.board = Board(dimx, dimy) self.board = Board(dimx, dimy)
coin1x = np.random.randint(0, dimx) self.board.tiles[3,3]['star'] = True
coin1y = np.random.randint(0, dimy)
self.board.tiles[coin1y,coin1x]['star'] = True
self.board.tiles[3,2]['star'] = True self.board.tiles[3,2]['star'] = True
# TODO fix number of commands at 5 # TODO fix number of commands at 5
self.cmds = [Command('forward'), Command('left', color=RED), Command('left', color=BLUE), Command('P0'), Command('-')] self.cmds = [Command('forward'), Command('left', color=RED), Command('left', color=BLUE), Command('P0'), Command('-')]
self.state = 'reset' self.state = 'input'
self.prg = Program(self.robot, self.board, self.cmds) self.prg = Program(self.robot, self.board, self.cmds)
@ -301,7 +298,6 @@ class Game:
self.won_text = myfont.render('YOU WON', False, GREEN) self.won_text = myfont.render('YOU WON', False, GREEN)
self.run_text = myfont.render('RUN', False, tuple(BLACK)) self.run_text = myfont.render('RUN', False, tuple(BLACK))
self.stop_text = myfont_small.render('STOP', False, tuple(BLACK)) self.stop_text = myfont_small.render('STOP', False, tuple(BLACK))
self.step_text = myfont_small.render('STEP', False, tuple(BLACK))
# save initial state # save initial state
self.initial_pos = (self.robot.x, self.robot.y, self.robot.orientation) self.initial_pos = (self.robot.x, self.robot.y, self.robot.orientation)
@ -361,9 +357,6 @@ class Game:
elif self.state == 'running': elif self.state == 'running':
btn_surf.fill(tuple(RED)) btn_surf.fill(tuple(RED))
btn_surf.blit(self.stop_text, (0, 10)) btn_surf.blit(self.stop_text, (0, 10))
elif self.state == 'stepping':
btn_surf.fill(tuple(YELLOW))
btn_surf.blit(self.step_text, (0, 10))
self.screen.blit(btn_surf, self.btn_rect) self.screen.blit(btn_surf, self.btn_rect)
# render messages # render messages
@ -407,8 +400,6 @@ class Game:
self.state = 'input' self.state = 'input'
elif self.state == 'input': elif self.state == 'input':
self.state = 'running' self.state = 'running'
elif self.state == 'stepping':
self.state = self.prg.step(self.state)
elif event.type == pygame.KEYUP: elif event.type == pygame.KEYUP:
if event.key == pygame.K_x: if event.key == pygame.K_x:
if not self.beamer_mode: if not self.beamer_mode:
@ -423,17 +414,15 @@ class Game:
# switch to normal mode # switch to normal mode
os.environ['SDL_VIDEO_WINDOW_POS'] = '0, 0' os.environ['SDL_VIDEO_WINDOW_POS'] = '0, 0'
self.scale_fac = 125 self.scale_fac = 125
self.screen = pygame.display.set_mode((int(self.board.tiles.shape[1] * self.scale_fac * 1.1), self.screen = pygame.display.set_mode((self.board.tiles.shape[1] * self.scale_fac,
int((self.board.tiles.shape[0] + 2) * self.scale_fac * 1.2))) self.board.tiles.shape[0] * self.scale_fac + 5 * self.scale_fac))
self.beamer_mode = False self.beamer_mode = False
elif event.key == pygame.K_r: elif event.key == pygame.K_r:
# run program # run program
self.state = 'running' self.state = 'running'
elif event.key == pygame.K_s: elif event.key == pygame.K_s:
if self.state != 'stepping': self.state = 'manual'
self.state = 'stepping' self.prg.step()
else:
self.state = self.prg.step(self.state)
return self.state return self.state
def reset(self): def reset(self):
@ -441,7 +430,6 @@ class Game:
self.robot.x = self.initial_pos[0] self.robot.x = self.initial_pos[0]
self.robot.y = self.initial_pos[1] self.robot.y = self.initial_pos[1]
self.robot.orientation = self.initial_pos[2] self.robot.orientation = self.initial_pos[2]
self.robot.update_pos(self.board.get_xdims(), self.board.get_ydims())
self.board.tiles = self.inital_board_tiles.copy() self.board.tiles = self.inital_board_tiles.copy()
return 'input' return 'input'
@ -457,9 +445,7 @@ class Game:
self.state = self.reset() self.state = self.reset()
elif self.state == 'quit': elif self.state == 'quit':
running = False running = False
elif self.state == 'stepping': elif self.state == 'manual':
pass
elif self.state == 'game_over' or self.state == 'won':
pass pass
else: else:
print("unknown state") print("unknown state")

32
remote_control/aruco_estimator.py
View File

@ -61,7 +61,7 @@ class CornerMarkerGroup(parameterTypes.GroupParameter):
class ArucoEstimator: class ArucoEstimator:
def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=7, grid_rows=4): def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8):
self.app = QtGui.QApplication([]) self.app = QtGui.QApplication([])
## Create window with GraphicsView widget ## Create window with GraphicsView widget
@ -91,8 +91,8 @@ class ArucoEstimator:
self.fps_overlay = pg.TextItem('fps = 0', color=(255, 255, 0), anchor=(0,1)) self.fps_overlay = pg.TextItem('fps = 0', color=(255, 255, 0), anchor=(0,1))
self.plotwidget.addItem(self.fps_overlay) self.plotwidget.addItem(self.fps_overlay)
self.invert_grayscale = False self.invert_grayscale = True
self.draw_grid = True self.draw_grid = False
self.draw_markers = True self.draw_markers = True
self.draw_marker_coordinate_system = False self.draw_marker_coordinate_system = False
self.corner_marker_size = 0.075 self.corner_marker_size = 0.075
@ -413,7 +413,7 @@ class ArucoEstimator:
# checks if all robot markers have been detected at least once # checks if all robot markers have been detected at least once
return not any([estimate['t'] is None for estimate in self.robot_marker_estimates.values()]) return not any([estimate['t'] is None for estimate in self.robot_marker_estimates.values()])
def get_pos_from_grid_point(self, x, y, dimx, dimy, orientation=None): def get_pos_from_grid_point(self, x, y, orientation=None):
""" """
returns the position for the given grid point based on the current corner estimates returns the position for the given grid point based on the current corner estimates
:param x: x position on the grid ( 0 &le x &lt number of grid columns) :param x: x position on the grid ( 0 &le x &lt number of grid columns)
@ -422,21 +422,17 @@ class ArucoEstimator:
:return: numpy array with corresponding real world x- and y-position :return: numpy array with corresponding real world x- and y-position
if orientation was specified the array also contains the matching angle for the orientation if orientation was specified the array also contains the matching angle for the orientation
""" """
if not self.all_corners_detected(): assert 0 <= x < self.grid_columns
#raise RuntimeError("not all corner markers have been detected yet") assert 0 <= y < self.grid_rows
a = np.array([0,1]) assert self.all_corners_detected()
b = np.array([1,1])
c = np.array([1,0])
d = np.array([0,0])
else:
# compute column line
a = np.array([self.corner_estimates['a']['x'], self.corner_estimates['a']['y']])
b = np.array([self.corner_estimates['b']['x'], self.corner_estimates['b']['y']])
c = np.array([self.corner_estimates['c']['x'], self.corner_estimates['c']['y']])
d = np.array([self.corner_estimates['d']['x'], self.corner_estimates['d']['y']])
x_frac = (x + 0.5) / dimx # compute column line
y_frac = (y + 0.5) / dimy a = np.array([self.corner_estimates['a']['x'], self.corner_estimates['a']['y']])
b = np.array([self.corner_estimates['b']['x'], self.corner_estimates['b']['y']])
c = np.array([self.corner_estimates['c']['x'], self.corner_estimates['c']['y']])
d = np.array([self.corner_estimates['d']['x'], self.corner_estimates['d']['y']])
x_frac = (x + 0.5) / self.grid_columns
y_frac = (y + 0.5) / self.grid_rows
vab = b - a vab = b - a
vdc = c - d vdc = c - d

40
remote_control/control_commander.py
View File

@ -7,7 +7,7 @@ from mpc_controller import MPCController
from event_listener import EventListener from event_listener import EventListener
class ControlCommander: class CommanderBase:
valid_controller_types = {'pid': PIDController, valid_controller_types = {'pid': PIDController,
'mpc': MPCController} 'mpc': MPCController}
@ -26,17 +26,17 @@ class ControlCommander:
if type(controller_type) == dict: if type(controller_type) == dict:
for id, ctype in controller_type.items(): for id, ctype in controller_type.items():
if ctype in ControlCommander.valid_controller_types: if ctype in CommanderBase.valid_controller_types:
self.robots[id].attach_controller(ControlCommander.valid_controller_types[ctype]()) self.robots[id].attach_controller(CommanderBase.valid_controller_types[ctype]())
else: else:
raise Exception(f"invalid controller type {ctype} specified for robot {id}. " raise Exception(f"invalid controller type {ctype} specified for robot {id}. "
f"valid controller types are {list(ControlCommander.valid_controller_types.keys())}") f"valid controller types are {list(CommanderBase.valid_controller_types.keys())}")
elif controller_type in ControlCommander.valid_controller_types: elif controller_type in CommanderBase.valid_controller_types:
for id, r in self.robots.items(): for id, r in self.robots.items():
r.attach_controller(ControlCommander.valid_controller_types[controller_type]()) r.attach_controller(CommanderBase.valid_controller_types[controller_type]())
else: else:
raise Exception(f"invalid controller type {controller_type} specified. valid controller types are " raise Exception(f"invalid controller type {controller_type} specified. valid controller types are "
f"{list(ControlCommander.valid_controller_types.keys())}") f"{list(CommanderBase.valid_controller_types.keys())}")
self.event_listener = EventListener(event_server=('127.0.0.1', 42424)) self.event_listener = EventListener(event_server=('127.0.0.1', 42424))
@ -68,24 +68,14 @@ class ControlCommander:
def handle_event(self, event): def handle_event(self, event):
# handle events from opencv window # handle events from opencv window
print("event: ", event) print("event: ", event)
if event[0] == 'click': # non-blocking move to target pos if event[0] == 'click':
target = event[1] target = event[1]
target_pos = np.array([target['x'], target['y'], target['angle']]) target_pos = np.array([target['x'], target['y'], target['angle']])
controlled_robot_id = list(self.robots.keys())[self.current_robot_index] controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
self.robots[controlled_robot_id].move_to_pos(target_pos) self.robots[controlled_robot_id].move_to_pos(target_pos)
elif event[0] == 'move_blocking': # blocking move to specified target position
target = event[1]
target_pos = np.array([target['x'], target['y'], target['angle']])
controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
# initialize move and wait for the robot to reach the target position
self.robots[controlled_robot_id].move_to_pos(target_pos, blocking=True)
#time.sleep(0.5)
# send confirmation to the server which initiated the command
self.event_listener.send_reply("ack\n".encode())
elif event[0] == 'key': elif event[0] == 'key':
key = event[1] key = event[1]
if key == 16777235: # arrow up if key == 32: # arrow up
self.controlling = not self.controlling self.controlling = not self.controlling
if not self.controlling: if not self.controlling:
print("disable control") print("disable control")
@ -95,7 +85,7 @@ class ControlCommander:
print("enable control") print("enable control")
for _, r in self.robots.items(): for _, r in self.robots.items():
r.start_control() r.start_control()
elif key == 16777236: # left elif key == 9: # TAB
# switch controlled robot # switch controlled robot
self.current_robot_index = (self.current_robot_index + 1) % len(self.robots) self.current_robot_index = (self.current_robot_index + 1) % len(self.robots)
controlled_robot_id = list(self.robots.keys())[self.current_robot_index] controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
@ -111,13 +101,13 @@ class ControlCommander:
if __name__ == '__main__': if __name__ == '__main__':
id_ip_dict = { id_ip_dict = {
#11: '10.10.11.88', #11: '10.10.11.88',
12: '192.168.1.12', #12: '10.10.11.91',
13: '192.168.1.13', #13: '10.10.11.90',
#14: '10.10.11.89', 14: '10.10.11.89',
} }
# controller_type = {12: 'mpc', 13: 'pid'} # controller_type = {12: 'mpc', 13: 'pid'}
controller_type = 'pid' controller_type = 'mpc'
rc = ControlCommander(id_ip_dict, controller_type=controller_type) rc = CommanderBase(id_ip_dict, controller_type=controller_type)
rc.run() rc.run()

3
remote_control/event_listener.py
View File

@ -34,9 +34,6 @@ class EventListener:
except ConnectionRefusedError: except ConnectionRefusedError:
print(f"error: could not connect to event server at {event_server}.") print(f"error: could not connect to event server at {event_server}.")
def send_reply(self, data):
self.event_socket.sendall(data)
def receive_events(self): def receive_events(self):
self.receiving = True self.receiving = True
while self.receiving: while self.receiving:

51
remote_control/measurement_server.py
View File

@ -2,7 +2,6 @@ import socketserver
import threading import threading
import time import time
import json import json
from queue import Queue
from aruco_estimator import ArucoEstimator from aruco_estimator import ArucoEstimator
@ -16,23 +15,11 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
if 'events' in data.decode(): if 'events' in data.decode():
self.request.sendall('subscribed to events\n'.encode()) self.request.sendall('subscribed to events\n'.encode())
# send any events in the event queue to the subscriber # send input events
while True: while True:
while not self.server.estimator.event_queue.empty(): while not self.server.estimator.event_queue.empty():
event = self.server.estimator.event_queue.get() event = self.server.estimator.event_queue.get()
# we distinguish two kinds of events: self.request.sendall((json.dumps(event) + '\n').encode())
if event[0] == 'response_event':
# 1. for 'response_event' events we expect the event subscriber to give a reply which will then
# by passed on to the response queue for transmitting to the original correspondent
message = event[1]['event']
#print(f"passing command {message} on to subscriber")
self.request.sendall((json.dumps(message) + '\n').encode())
reply = self.request.recv(1024)
#print(f"putting reply {reply} in response queue")
self.server.response_queue.put(reply)
else:
# 2. for other types of events we don't expect a reply and just pass them on to the subscriber
self.request.sendall((json.dumps(event) + '\n').encode())
self.server.estimator.last_event = None self.server.estimator.last_event = None
time.sleep(1.0 / self.server.max_measurements_per_second) time.sleep(1.0 / self.server.max_measurements_per_second)
elif 'corners' in data.decode(): elif 'corners' in data.decode():
@ -42,34 +29,6 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
for corner, data in corner_estimates.items(): for corner, data in corner_estimates.items():
response[corner] = {'x': data['x'], 'y': data['y']} response[corner] = {'x': data['x'], 'y': data['y']}
self.request.sendall((json.dumps(response) + '\n').encode()) self.request.sendall((json.dumps(response) + '\n').encode())
elif 'move_grid_blocking' in data.decode():
# if we receive a move_grid event
# ( e.g. move_grid;{"x":1,"y":1,"dimx":10,"dimy":10,"orientation":"^","require_response":True} )
# we compute the corresponding real-world position the robot should drive to
# and then create a new move event which is put in the event queue and will be propagated to the ControlCommander
data_decode = data.decode()
# print("data: ", data_decode)
payload = data.decode().split(';')[1]
grid_pos = json.loads(payload)
# print("grid_pos = ", grid_pos)
pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['dimx'],
grid_pos['dimy'], grid_pos['orientation'])
# print("pos = ", pos)
# print("event requiring response")
# put blocking move command in event queue
self.server.estimator.event_queue.put(('response_event',
{'event': ('move_blocking', {'x': pos[0], 'y': pos[1], 'angle': pos[2]})}))
# wait for response of the move command
# TODO this assumes that we wait only for at most one response at a time
# we could add some kind of reference here to handle multiple responses (e.g. id of the response to wait for)
while self.server.response_queue.empty():
pass
reply = self.server.response_queue.get()
# send back response to the original source
#print(f"sending reply {reply} back to correspondent {self.request}")
self.request.sendall(reply)
else: else:
# send robot position # send robot position
try: try:
@ -98,18 +57,16 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
super().__init__(server_address, RequestHandlerClass) super().__init__(server_address, RequestHandlerClass)
self.estimator = estimator self.estimator = estimator
self.max_measurements_per_second = max_measurements_per_second self.max_measurements_per_second = max_measurements_per_second
self.response_queue = Queue()
def handle_error(self, request, client_address): def handle_error(self, request, client_address):
print("an error occurred -> terminating connection") print("an error occurred -> terminating connection")
if __name__ == "__main__": if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[12, 13]) aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[12, 13])
# first we start thread for the measurement server # first we start thread for the measurement server
measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, max_measurements_per_second=30)
max_measurements_per_second=30)
server_thread = threading.Thread(target=measurement_server.serve_forever) server_thread = threading.Thread(target=measurement_server.serve_forever)
server_thread.start() server_thread.start()