implemented blocking events to make it possible to wait for the robot to reach the desired target position

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:
+                if ctype in ControlCommander.valid_controller_types:
-                    self.robots[id].attach_controller(CommanderBase.valid_controller_types[ctype]())
+                    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())}")
+                                    f"valid controller types are {list(ControlCommander.valid_controller_types.keys())}")
-        elif controller_type in CommanderBase.valid_controller_types:
+        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,11 +68,21 @@ 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 == 16777235:  # arrow up
@@ -109,5 +119,5 @@ if __name__ == '__main__':
     # controller_type = {12: 'mpc', 13: 'pid'}
     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,11 +16,23 @@ 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()
-                    self.request.sendall((json.dumps(event) + '\n').encode())
+                    # 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)
         elif 'corners' in data.decode():
@@ -29,14 +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' in data.decode():
+        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)
+#            print("data: ", data_decode)
             payload = data.decode().split(';')[1]
             grid_pos = json.loads(payload)
-            print("grid_pos = ", grid_pos)
+#            print("grid_pos = ", grid_pos)
-            pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['orientation'])
+            pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['dimx'],
-            self.server.estimator.event_queue.put(('click', {'x': pos[0], 'y': pos[1], 'angle': pos[2]}))
+                                                                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:
@@ -65,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()