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