Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

general refactoring

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-10-24 21:16:17 +02:00
parent 8728db82e8
commit 119d857531
2 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
remote_control/measurement_server.py
View File

@ -4,6 +4,7 @@ import time
from aruco_estimator import ArucoEstimator
class MeasurementHandler(socketserver.BaseRequestHandler):
def handle(self) -> None:
data = self.request[0]
@ -11,11 +12,12 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
cur_thread = threading.current_thread()
print(f"current thread {cur_thread}")
try:
id = int(data)
if id in self.server.estimator.robot_marker_estimates:
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[id]}\n".encode(), self.client_address)
time.sleep(1.0/self.server.max_measurements_per_second)
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)
@ -24,18 +26,21 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
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)
aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[15])
estimator_thread = threading.Thread(target=aruco_estimator.run_tracking)
estimator_thread.start()
with MeasurementServer(('127.0.0.1', 42424), MeasurementHandler, estimator, max_measurements_per_second=30) as measurement_server:
with MeasurementServer(('127.0.0.1', 42424), MeasurementHandler, aruco_estimator,
max_measurements_per_second=30) as measurement_server:
measurement_server.serve_forever()
# receive with: nc 127.0.0.1 42424 -u -> 15 + Enter