implemented communication interface with real robots

gauss-turing/webserver/event_server_comm.py

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

gauss-turing/webserver/gauss_turing.py

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

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=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

remote_control/control_commander.py

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

remote_control/event_listener.py

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

remote_control/measurement_server.py

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