Compare commits

...

7 Commits

6 changed files with 144 additions and 55 deletions

View File

@ -0,0 +1,15 @@
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)

View File

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

View File

@ -61,7 +61,7 @@ class CornerMarkerGroup(parameterTypes.GroupParameter):
class ArucoEstimator:
def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=8, grid_rows=8):
def __init__(self, robot_marker_ids=None, use_realsense=True, grid_columns=7, grid_rows=4):
self.app = QtGui.QApplication([])
## 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.plotwidget.addItem(self.fps_overlay)
self.invert_grayscale = True
self.draw_grid = False
self.invert_grayscale = False
self.draw_grid = True
self.draw_markers = True
self.draw_marker_coordinate_system = False
self.corner_marker_size = 0.075
@ -413,7 +413,7 @@ class ArucoEstimator:
# 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()])
def get_pos_from_grid_point(self, x, y, orientation=None):
def get_pos_from_grid_point(self, x, y, dimx, dimy, orientation=None):
"""
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)
@ -422,17 +422,21 @@ class ArucoEstimator:
: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
"""
assert 0 <= x < self.grid_columns
assert 0 <= y < self.grid_rows
assert self.all_corners_detected()
if not self.all_corners_detected():
#raise RuntimeError("not all corner markers have been detected yet")
a = np.array([0,1])
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) / self.grid_columns
y_frac = (y + 0.5) / self.grid_rows
x_frac = (x + 0.5) / dimx
y_frac = (y + 0.5) / dimy
vab = b - a
vdc = c - d

View File

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

View File

@ -34,6 +34,9 @@ class EventListener:
except ConnectionRefusedError:
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):
self.receiving = True
while self.receiving:

View File

@ -2,6 +2,7 @@ import socketserver
import threading
import time
import json
from queue import Queue
from aruco_estimator import ArucoEstimator
@ -15,10 +16,22 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
if 'events' in data.decode():
self.request.sendall('subscribed to events\n'.encode())
# send input events
# send any events in the event queue to the subscriber
while True:
while not self.server.estimator.event_queue.empty():
event = self.server.estimator.event_queue.get()
# we distinguish two kinds of events:
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
time.sleep(1.0 / self.server.max_measurements_per_second)
@ -29,6 +42,34 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
for corner, data in corner_estimates.items():
response[corner] = {'x': data['x'], 'y': data['y']}
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:
# send robot position
try:
@ -57,16 +98,18 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
super().__init__(server_address, RequestHandlerClass)
self.estimator = estimator
self.max_measurements_per_second = max_measurements_per_second
self.response_queue = Queue()
def handle_error(self, request, client_address):
print("an error occurred -> terminating connection")
if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[12, 13])
aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[12, 13])
# first we start thread for the measurement server
measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, max_measurements_per_second=30)
measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator,
max_measurements_per_second=30)
server_thread = threading.Thread(target=measurement_server.serve_forever)
server_thread.start()