implemented control of multiple robots through GUI using measurement server

remote_control/aruco_estimator.py

@@ -141,6 +141,7 @@ class ArucoEstimator:
             {'name': 'Grid rows', 'type': 'int', 'value': self.grid_rows, 'tip': "Number of rows for the grid"},
             {'name': 'Display mode', 'type': 'list', 'values': ['color', 'grayscale'], 'value': 'color', 'tip': "Display mode for the video"},
             {'name': 'Autoexposure', 'type': 'bool', 'value': True},
+            {'name': 'Controlled robot', 'type': 'list', 'values': self.robot_marker_ids, 'tip': 'Robot to control'},
             RobotMarkerGroup(name="Robot markers", children=robot_marker_group),
             CornerMarkerGroup(name="Corner markers", children=corner_marker_group),
             ]
@@ -156,6 +157,8 @@ class ArucoEstimator:
         self.params.param('Robot marker size').sigValueChanged.connect(lambda _, v: self.__setattr__('robot_marker_size', v))
         self.params.param('Show FPS').sigValueChanged.connect(lambda _, v: self.fps_overlay.show() if v else self.fps_overlay.hide())
         self.params.param('Autoexposure').sigValueChanged.connect(lambda _, v: self.set_autoexposure(v))
+        self.params.param('Controlled robot').sigValueChanged.connect(lambda _, v: self.event_queue.put(('controlled_robot',
+                                                                                                         {'robot_id': v})))
 
         self.paramtree = ParameterTree()
         self.paramtree.setParameters(self.params, showTop=False)

remote_control/casadi_opt.py

@@ -143,7 +143,7 @@ class OpenLoopSolver:
 
 
     def solve(self, x0, target):
-
+        target = np.asarray(target)
         angles_unwrapped = np.unwrap([x0[2], target[2]])  # unwrap angle to avoid jump in data
         x0[2] = angles_unwrapped[0]
         target[2] = angles_unwrapped[1]

remote_control/mpc_test.py

@@ -2,25 +2,12 @@ import sys
 import socket
 import threading
 import time
+import json
 
 from robot import Robot, ControlledRobot
 
 from mpc_controller import MPCController
 
-# HOST, PORT = "localhost", 42424
-#
-# robot_id = 12
-#
-# sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-# sock.connect((HOST, PORT))
-# sock.sendall(f"{robot_id}\n".encode())     # request data for robot with given id
-# #sock.sendall(f"events\n".encode())        # request events
-# receiving = True
-# while receiving:
-#     received = str(sock.recv(1024), "utf-8")
-#     print("Received: {}".format(received))
-#     receiving = len(received) > 0
-
 
 class RemoteController:
     def __init__(self):
@@ -28,35 +15,84 @@ class RemoteController:
         #self.robots = [Robot(11, '192.168.1.11')] #, Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
         #self.robots = [Robot(12, '10.10.11.91'), Robot(13, '10.10.11.90'), Robot(14, '10.10.11.89')]
         #self.robots = [Robot(14, '10.10.11.89')]
-        self.robots = [ControlledRobot(13, '192.168.1.13')]
+        self.robots = [ControlledRobot(13, '192.168.1.13'), ControlledRobot(12, '192.168.1.12')]
 
         self.robot_ids = {}
         for r in self.robots:
             self.robot_ids[r.id] = r
-
+        self.controlled_robot = self.robots[0].id
-        # socket for movement commands
-        self.comm_socket = socket.socket()
-        self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
 
         for robot in self.robots:
             robot.connect()
 
-        self.comm_socket.bind(('', 1337))
+        # thread for handling events received by measurement server (from GUI)
-        self.comm_socket.listen(5)
+        self.event_thread = threading.Thread(target=self.handle_events)
+        self.event_thread.daemon = True
+
+        # connect to socket for events from GUI
+        try:
+            HOST, PORT = "localhost", 42424
+            self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+            self.event_socket.connect((HOST, PORT))
+            self.event_socket.sendall("events\n".encode())
+            self.event_socket.settimeout(0.1)
+            # check if we receive data from the measurement server
+            response = self.event_socket.recv(1024)
+            if 'error' not in str(response):
+                print("... connected! -> start listening for measurements")
+                self.event_socket.settimeout(None)
+                # if so we start the measurement thread
+                self.event_thread.start()
+            else:
+                print(f"error: cannot communicate with the measurement server.\n       The response was: {response}")
+        except socket.timeout:
+            print(f"error: the measurement server did not respond with data.")
+
+
+        #sock.sendall(f"{robot_id}\n".encode())     # request data for robot with given id
+        # sock.sendall(f"events\n".encode())        # request events
+        # receiving = True
+        # while receiving:
+        #     received = str(sock.recv(1024), "utf-8")
+        #     print("Received: {}".format(received))
+        #     receiving = len(received) > 0
 
         self.t = time.time()
 
         # start thread for marker position detection
-        self.controller = MPCController()
+        self.controllers = []
-        self.robots[0].attach_controller(self.controller)
+        for r in self.robots:
+            c = MPCController()
+            self.controllers.append(c)
+            r.attach_controller(c)
+
+    def handle_events(self):
+        receiving = True
+        while receiving:
+            received = str(self.event_socket.recv(1024), "utf-8")
+            if len(received) > 0:
+                last_received = received.split('\n')[-2]
+                event_type, payload = json.loads(last_received)
+                print(event_type, payload)
+
+                if event_type == 'click':
+                    target_pos = (payload['x'], payload['y'], payload['angle'])
+                    self.robot_ids[self.controlled_robot].move_to_pos(target_pos=target_pos)
+                elif event_type == 'controlled_robot':
+                    self.controlled_robot = payload['robot_id']
+            else:
+                receiving = False
+                print(f"measurement server stopped sending event data")
 
     def run(self):
+
         #print("waiting until all markers are detected...")
         #while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()):
         #    pass
         print("starting control")
         #self.controller.interactive_control(robots=self.robots)
-        self.controller.start()
+        for c in self.controllers:
+            c.start()
         pass
 
 
@@ -70,10 +106,9 @@ if __name__ == '__main__':
 
 # to use the program, first start measurement_server.py, which launches the Aruco marker detection GUI and streams
 # marker positions and GUI events via socket
-# next, launch this program (mpc_test.py) in debug mode and set a breakpoint after self.controller.start()
+# next, launch this program (mpc_test.py)
-# you can now set the target position for the controlled robot via move_to_pos(), e.g.
+# you can now direct the robots via the GUI (assuming four corner markers for position reference have been detected)
-# self.robots[0].move_to_pos([0.0,0,0], True)
+# click any position on the screen and the current robot will drive there
+# you can also switch which robot should be controlled in the GUI
 
-# TODO
+# TODO document and improve program structure
-# figure out how to get clicked positions from measurement server and drive robot there
-# also document and improve program structure