gauss-turing/webserver/event_server_comm.py

@@ -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)

gauss-turing/webserver/gauss_turing.py

@@ -3,8 +3,6 @@ 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)
@@ -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)])
 
 
-
 class Board:
     valid_colors = [WHITE, RED, BLUE]
 
@@ -32,6 +29,8 @@ 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):
@@ -52,12 +51,6 @@ 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]):
@@ -110,9 +103,6 @@ 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}"
 
@@ -193,7 +183,7 @@ class Program:
         self.prg_counter = 0
         self.screen_rect = None
 
-    def step(self, state='running'):
+    def step(self):
         cmd = self.cmds[self.prg_counter]
         self.prg_counter += 1
 
@@ -211,15 +201,24 @@ 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
@@ -236,7 +235,7 @@ class Program:
             return 'won'
 
         # by default we continue in the running state
-        return state
+        return 'running'
 
     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
@@ -278,14 +277,12 @@ class Game:
     def __init__(self, dimx, dimy, robotx, roboty):
         self.robot = Robot(x=robotx, y=roboty, orientation='v')
         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
 
         # TODO fix number of commands at 5
         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)
 
@@ -301,7 +298,6 @@ 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)
@@ -361,9 +357,6 @@ 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
@@ -407,8 +400,6 @@ 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:
@@ -423,17 +414,15 @@ class Game:
                         # switch to normal mode
                         os.environ['SDL_VIDEO_WINDOW_POS'] = '0, 0'
                         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
                 elif event.key == pygame.K_r:
                     # run program
                     self.state = 'running'
                 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
 
     def reset(self):
@@ -441,7 +430,6 @@ 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'
 
@@ -457,9 +445,7 @@ class Game:
                 self.state = self.reset()
             elif self.state == 'quit':
                 running = False
-            elif self.state == 'stepping':
+            elif self.state == 'manual':
-                pass
-            elif self.state == 'game_over' or self.state == 'won':
                 pass
             else:
                 print("unknown state")

remote_control/aruco_estimator.py

@@ -61,7 +61,7 @@ class CornerMarkerGroup(parameterTypes.GroupParameter):
 
 
 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([])
 
         ## 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 = False
+        self.invert_grayscale = True
-        self.draw_grid = True
+        self.draw_grid = False
         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, 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
         :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
                     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) / self.grid_columns
-        x_frac = (x + 0.5) / dimx
+        y_frac = (y + 0.5) / self.grid_rows
-        y_frac = (y + 0.5) / dimy
 
         vab = b - a
         vdc = c - d

remote_control/control_commander.py

@@ -7,7 +7,7 @@ from mpc_controller import MPCController
 from event_listener import EventListener
 
 
-class ControlCommander:
+class CommanderBase:
     valid_controller_types = {'pid': PIDController,
                               'mpc': MPCController}
 
@@ -26,17 +26,17 @@ class ControlCommander:
 
         if type(controller_type) == dict:
             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:
                     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():
-                r.attach_controller(ControlCommander.valid_controller_types[controller_type]())
+                r.attach_controller(CommanderBase.valid_controller_types[controller_type]())
         else:
             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))
 
@@ -68,24 +68,14 @@ class ControlCommander:
     def handle_event(self, event):
         # handle events from opencv window
         print("event: ", event)
-        if event[0] == 'click': # non-blocking move to target pos
+        if event[0] == 'click':
             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 == 16777235:  # arrow up
+            if key == 32:  # arrow up
                 self.controlling = not self.controlling
                 if not self.controlling:
                     print("disable control")
@@ -95,7 +85,7 @@ class ControlCommander:
                     print("enable control")
                     for _, r in self.robots.items():
                         r.start_control()
-            elif key == 16777236:  # left
+            elif key == 9:  # TAB
                 # 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]
@@ -111,13 +101,13 @@ class ControlCommander:
 if __name__ == '__main__':
     id_ip_dict = {
         #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 = 'pid'
+    controller_type = 'mpc'
 
-    rc = ControlCommander(id_ip_dict, controller_type=controller_type)
+    rc = CommanderBase(id_ip_dict, controller_type=controller_type)
     rc.run()

remote_control/event_listener.py

@@ -34,9 +34,6 @@ 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:

remote_control/measurement_server.py

@@ -2,7 +2,6 @@ import socketserver
 import threading
 import time
 import json
-from queue import Queue
 
 from aruco_estimator import ArucoEstimator
 
@@ -16,22 +15,10 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
 
         if 'events' in data.decode():
             self.request.sendall('subscribed to events\n'.encode())
-            # send any events in the event queue to the subscriber
+            # send input events
             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)
@@ -42,34 +29,6 @@ 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:
@@ -98,18 +57,16 @@ 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=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
-    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.start()