implemented control from multiple robots

remote_control/controller.py

@@ -0,0 +1,56 @@
+import threading
+import time
+
+class ControllerBase:
+    def __init__(self, control_rate=0.1):
+        self.robot = None
+        self.controlling = False
+        self.target_pos = None
+
+        self.control_thread = None
+        self.control_rate = control_rate
+
+    def control_loop(self):
+        while self.controlling:
+            if self.target_pos is not None:
+                state = self.get_measured_position()
+
+                control = self.compute_control(state)
+
+                self.apply_control(control)
+
+                time.sleep(self.control_rate)
+        self.apply_control((0.0,0.0))   # stop robot
+
+    def set_target_position(self, target_pos):
+        self.target_pos = target_pos
+
+    def get_measured_position(self):
+        if self.robot is not None:
+            return self.robot.get_measurement()
+        else:
+            raise Exception("error: controller cannot get measurement!\n       there is no robot attached to the controller!")
+
+    def compute_control(self, state):
+        return (0.0, 0.0)
+
+    def apply_control(self, control):
+        if self.robot is not None:
+            self.robot.send_cmd(u1=control[0], u2=control[1])
+        else:
+            raise Exception("error: controller cannot apply control!\n       there is no robot attached to the controller!")
+
+    def attach_robot(self, robot):
+        self.robot = robot
+
+    def start(self):
+        self.controlling = True
+        # start control thread
+        self.control_thread = threading.Thread(target=self.control_loop)
+        self.control_thread.start()
+
+    def stop(self):
+        # pause controller
+        self.controlling = False
+        if self.control_thread is not None:
+            self.control_thread.join()

remote_control/event_listener.py

@@ -7,7 +7,9 @@ import json
 class EventListener:
     def __init__(self, event_server):
         self.event_thread = threading.Thread(target=self.receive_events)
+        self.event_thread.daemon = True  # mark thread as daemon -> it terminates automatically when program shuts down
         self.event_queue = queue.Queue()
+        self.receiving = False
 
         # connect to event server
         print(f"connecting to event server on {event_server} ...")
@@ -33,8 +35,8 @@ class EventListener:
             print(f"error: could not connect to event server at {event_server}.")
 
     def receive_events(self):
-        receiving = True
+        self.receiving = True
-        while receiving:
+        while self.receiving:
             received = str(self.event_socket.recv(1024), "utf-8")
             if len(received) > 0:
                 events = received.split('\n')
@@ -43,5 +45,5 @@ class EventListener:
                         event = json.loads(event_json)
                         self.event_queue.put(event)
             else:
-                receiving = False
+                self.receiving = False
         print("event server seems to have shut down -> stop listening")

remote_control/measurement_server.py

@@ -50,7 +50,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
         print("an error occurred -> terminating connection")
 
 if __name__ == "__main__":
-    aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[11, 12, 13, 14])
+    aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14])
     estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True})
     estimator_thread.start()
 

remote_control/robot.py

@@ -13,6 +13,7 @@ class Robot:
         self.angle = None
 
         self.ip = ip
+        self.port = 1234
         self.socket = socket.socket()
 
         # currently active control
@@ -24,17 +25,19 @@ class Robot:
         self.measurement_server = measurement_server
         self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # TCP socket
         self.measurement_thread = threading.Thread(target=self.receive_measurements)
+        self.measurement_thread.daemon = True # mark thread as daemon -> it terminates automatically when program shuts down
+        self.receiving = False
 
 
     def connect(self):
         # connect to robot
         try:
-            print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
+            print(f"connecting to robot {self.ip} at {self.ip}:{self.port} ...")
-            self.socket.connect((self.ip, 1234))  # connect to robot
+            self.socket.connect((self.ip, self.port))  # connect to robot
             print("connected!")
             self.connected = True
         except socket.error:
-            print("error: could not connect to robot {} with ip {}".format(self.id, self.ip))
+            print(f"error: could not connect to robot {self.id} at {self.ip}:{self.port}")
 
         # connect to measurement server
         print(f"connecting to measurement server on {self.measurement_server} ...")
@@ -47,7 +50,7 @@ class Robot:
             # check if we receive data from the measurement server
             response = self.measurement_socket.recv(1024)
             if not 'error' in str(response):
-                print("... connected! -> start listening for events")
+                print("... connected! -> start listening for measurements")
                 self.measurement_socket.settimeout(None)
                 # if so we start the measurement thread
                 self.measurement_thread.start()
@@ -58,9 +61,8 @@ class Robot:
         except ConnectionRefusedError:
             print(f"error: could not connect to measurement server at {self.measurement_server}.")
 
-
     def send_cmd(self, u1=0.0, u2=0.0):
-        if self.socket:
+        if self.socket and self.connected:
             try:
                 self.socket.send(f'({u1},{u2})\n'.encode())
             except BrokenPipeError:
@@ -69,20 +71,64 @@ class Robot:
             except ConnectionResetError:
                 print(f"error: connection to robot {self.id} with ip {self.ip} lost")
                 pass
+        else:
+            print(f"error: robot {self.id} is not connected to {self.ip}")
 
     def receive_measurements(self):
-        receiving = True
+        self.receiving = True
-        while receiving:
+        while self.receiving:
             received = str(self.measurement_socket.recv(1024), "utf-8")
             if len(received) > 0:
-                measurement = json.loads(received)
+                last_received = received.split('\n')[-2]
+                measurement = json.loads(last_received)
                 self.t_last_measurement = measurement['t']
                 self.x = measurement['x']
                 self.y = measurement['y']
                 self.angle = measurement['angle']
             else:
-                receiving = False
+                self.receiving = False
                 print(f"measurement server stopped sending data for robot {self.id}")
 
+    def stop_measurement_thread(self):
+        self.receiving = False
+        self.measurement_thread.join()
+
     def get_measurement(self):
         return (self.t_last_measurement, self.x, self.y, self.angle)
+
+    def __str__(self):
+        connection_state = '' if self.connected else 'not'
+        state = self.get_measurement()
+        last_measurement = f'last measurement = {state}' if not None in state else 'no measurements available'
+        return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement
+
+    def __repr__(self):
+        return str(self)
+
+
+class ControlledRobot(Robot):
+    def __init__(self, marker_id, ip):
+        super().__init__(marker_id, ip)
+        self.controller = None
+
+    def start_control(self):
+        if self.controller is not None:
+            self.controller.start()
+        else:
+            raise Exception("Error: Cannot start control: there is not controller attached to the robot!")
+
+    def stop_control(self):
+        if self.controller is not None:
+            self.controller.stop()
+        else:
+            raise Exception("Error: Cannot stop control: there is not controller attached to the robot!")
+
+    def attach_controller(self, controller):
+        self.controller = controller
+        self.controller.attach_robot(self)
+
+    def move_to_pos(self, target_pos):
+        if self.controller is not None:
+            self.controller.set_target_position(target_pos)
+        else:
+            raise Exception("Error: Cannot move to position: there is not controller attached to the robot!")