From 3bd11a53ce087d743d05034ef22a5eb8c13470c8 Mon Sep 17 00:00:00 2001
From: Simon Pirkelmann <simon.pirkelmann@gmail.com>
Date: Sat, 24 Oct 2020 19:56:18 +0200
Subject: [PATCH] client and server for robot position estimate

---
 remote_control/measurement_client.py | 13 +++++++++
 remote_control/measurement_server.py | 41 ++++++++++++++++++++++++++++
 2 files changed, 54 insertions(+)
 create mode 100644 remote_control/measurement_client.py
 create mode 100644 remote_control/measurement_server.py

diff --git a/remote_control/measurement_client.py b/remote_control/measurement_client.py
new file mode 100644
index 0000000..810fada
--- /dev/null
+++ b/remote_control/measurement_client.py
@@ -0,0 +1,13 @@
+import socket
+
+HOST, PORT = "localhost", 42424
+
+robot_id = 15
+
+# SOCK_DGRAM is the socket type to use for UDP sockets
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+sock.sendto(f"{robot_id}\n".encode(), (HOST, PORT))
+while True:
+    received = str(sock.recv(1024), "utf-8")
+    print("Received: {}".format(received))
\ No newline at end of file
diff --git a/remote_control/measurement_server.py b/remote_control/measurement_server.py
new file mode 100644
index 0000000..d6a4cd1
--- /dev/null
+++ b/remote_control/measurement_server.py
@@ -0,0 +1,41 @@
+import socketserver
+import threading
+import time
+
+from aruco_estimator import ArucoEstimator
+
+class MeasurementHandler(socketserver.BaseRequestHandler):
+    def handle(self) -> None:
+        data = self.request[0]
+        socket = self.request[1]
+        cur_thread = threading.current_thread()
+        print(f"current thread {cur_thread}")
+        try:
+            id = int(data)
+            if id in self.server.estimator.robot_marker_estimates:
+                while True:
+                    socket.sendto(f"{self.server.estimator.robot_marker_estimates[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)
+
+        return
+
+class MeasurementServer(socketserver.ThreadingMixIn, socketserver.UDPServer):
+    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
+
+if __name__ == "__main__":
+    estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15])
+    estimator_thread = threading.Thread(target=estimator.run_tracking)
+    estimator_thread.start()
+
+    with MeasurementServer(('127.0.0.1', 42424), MeasurementHandler, estimator, max_measurements_per_second=30) as measurement_server:
+        measurement_server.serve_forever()
+
+    # receive with: nc 127.0.0.1 42424 -u  -> 15 + Enter
\ No newline at end of file