switched to using TCP sockets for measurement and event communication

remote_control/event_listener.py

@@ -0,0 +1,47 @@
+import socket
+import threading
+import queue
+import json
+
+
+class EventListener:
+    def __init__(self, event_server):
+        self.event_thread = threading.Thread(target=self.receive_events)
+        self.event_queue = queue.Queue()
+
+        # connect to event server
+        print(f"connecting to event server on {event_server} ...")
+        self.event_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # TCP socket
+
+        try:
+            self.event_socket.connect(event_server)
+            self.event_socket.sendall(f"events\n".encode())
+
+            self.event_socket.settimeout(0.1)
+            # check if we receive data from the event server
+            response = self.event_socket.recv(1024)
+            if not 'error' in str(response):
+                print("... connected! -> start listening for events")
+                self.event_socket.settimeout(None)
+                # if so we start the event thread
+                self.event_thread.start()
+            else:
+                print(f"error: cannot communicate with the event server.\n       The response was: {response}")
+        except socket.timeout:
+            print(f"error: the event server did not respond.")
+        except ConnectionRefusedError:
+            print(f"error: could not connect to event server at {event_server}.")
+
+    def receive_events(self):
+        receiving = True
+        while receiving:
+            received = str(self.event_socket.recv(1024), "utf-8")
+            if len(received) > 0:
+                events = received.split('\n')
+                for event_json in events:
+                    if len(event_json) > 0:
+                        event = json.loads(event_json)
+                        self.event_queue.put(event)
+            else:
+                receiving = False
+        print("event server seems to have shut down -> stop listening")

remote_control/measurement_client.py

@@ -2,12 +2,14 @@ import socket
 
 HOST, PORT = "localhost", 42424
 
-robot_id = 11
+robot_id = 12
 
-# SOCK_DGRAM is the socket type to use for UDP sockets
+sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.connect((HOST, PORT))
-
+sock.sendall(f"{robot_id}\n".encode())     # request data for robot with given id
-sock.sendto(f"{robot_id}\n".encode(), (HOST, PORT))
+#sock.sendall(f"events\n".encode())        # request events
-while True:
+receiving = True
+while receiving:
     received = str(sock.recv(1024), "utf-8")
     print("Received: {}".format(received))
+    receiving = len(received) > 0

remote_control/measurement_server.py

@@ -1,41 +1,56 @@
 import socketserver
 import threading
 import time
+import json
 
 from aruco_estimator import ArucoEstimator
 
 
 class MeasurementHandler(socketserver.BaseRequestHandler):
     def handle(self) -> None:
-        data = self.request[0]
+        self.data = self.request.recv(1024).strip()
-        socket = self.request[1]
+
         cur_thread = threading.current_thread()
         print(f"current thread {cur_thread}")
-        try:
-            marker_id = int(data)
-            if marker_id in self.server.estimator.robot_marker_estimates:
-                while True:
-                    socket.sendto(f"{self.server.estimator.robot_marker_estimates[marker_id]}\n".encode(),
-                                  self.client_address)
-                    time.sleep(1.0 / self.server.max_measurements_per_second)
-            else:
-                socket.sendto("error: unknown robot marker id\n".encode(),
-                              self.client_address)
-        except ValueError:
-            socket.sendto("error: data not understood. expected robot marker id (int)\n".encode(), self.client_address)
 
+        if 'events' in self.data.decode():
+            self.request.sendall('subscribed to events\n'.encode())
+            # send input events
+            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())
+        else:
+            # send robot position
+            try:
+                marker_id = int(self.data)
+            except ValueError:
+                marker_id = None
+
+            if marker_id is not None:
+                if marker_id in self.server.estimator.robot_marker_estimates:
+                    while True:
+                        self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id]) + '\n').encode())
+                        time.sleep(1.0 / self.server.max_measurements_per_second)
+                else:
+                    self.request.sendall("error: unknown robot marker id\n".encode())
+            else:
+                self.request.sendall("error: data not understood. expected <robot marker id (int)> or 'events'\n".encode())
         return
 
 
-class MeasurementServer(socketserver.ThreadingMixIn, socketserver.UDPServer):
+class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
+    allow_reuse_address = True
     def __init__(self, server_address, RequestHandlerClass, estimator, max_measurements_per_second=30):
         super().__init__(server_address, RequestHandlerClass)
         self.estimator = estimator
         self.max_measurements_per_second = max_measurements_per_second
 
+    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=[11, 12, 13, 14])
+    aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[11, 12, 13, 14])
     estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True})
     estimator_thread.start()
 
@@ -43,4 +58,4 @@ if __name__ == "__main__":
                            max_measurements_per_second=30) as measurement_server:
         measurement_server.serve_forever()
 
-    # receive with: nc 127.0.0.1 42424 -u  -> 15 + Enter
+    # receive with: nc 127.0.0.1 42424  -> 12 + Enter

remote_control/robot.py

@@ -1,10 +1,16 @@
 import socket
+import threading
+import json
+
 
 class Robot:
-    def __init__(self, marker_id, ip):
+    def __init__(self, marker_id, ip, measurement_server=('127.0.0.1', 42424)):
         self.id = marker_id
-        self.pos = None
+
-        self.euler = None
+        self.t_last_measurement = None
+        self.x = None
+        self.y = None
+        self.angle = None
 
         self.ip = ip
         self.socket = socket.socket()
@@ -15,6 +21,11 @@ class Robot:
 
         self.connected = False
 
+        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)
+
+
     def connect(self):
         # connect to robot
         try:
@@ -23,7 +34,30 @@ class Robot:
             print("connected!")
             self.connected = True
         except socket.error:
-            print("could not connect to robot {} with ip {}".format(self.id, self.ip))
+            print("error: could not connect to robot {} with ip {}".format(self.id, self.ip))
+
+        # connect to measurement server
+        print(f"connecting to measurement server on {self.measurement_server} ...")
+
+        try:
+            self.measurement_socket.connect(self.measurement_server)
+            self.measurement_socket.sendall(f"{self.id}\n".encode())
+
+            self.measurement_socket.settimeout(0.1)
+            # 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")
+                self.measurement_socket.settimeout(None)
+                # if so we start the measurement thread
+                self.measurement_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.")
+        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:
@@ -35,3 +69,20 @@ class Robot:
             except ConnectionResetError:
                 print(f"error: connection to robot {self.id} with ip {self.ip} lost")
                 pass
+
+    def receive_measurements(self):
+        receiving = True
+        while receiving:
+            received = str(self.measurement_socket.recv(1024), "utf-8")
+            if len(received) > 0:
+                measurement = json.loads(received)
+                self.t_last_measurement = measurement['t']
+                self.x = measurement['x']
+                self.y = measurement['y']
+                self.angle = measurement['angle']
+            else:
+                receiving = False
+                print(f"measurement server stopped sending data for robot {self.id}")
+
+    def get_measurement(self):
+        return (self.t_last_measurement, self.x, self.y, self.angle)