RoboRally/remote_control/measurement_server.py

94 lines
3.9 KiB
Python
Raw Normal View History

import socketserver
import threading
import time
import json
from aruco_estimator import ArucoEstimator
2020-10-24 19:16:17 +00:00
class MeasurementHandler(socketserver.BaseRequestHandler):
def handle(self) -> None:
2020-11-17 20:23:22 +00:00
data = self.request.recv(1024).strip()
cur_thread = threading.current_thread()
print(f"current thread {cur_thread}")
2020-11-17 20:23:22 +00:00
if 'events' in 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())
2020-11-14 15:40:33 +00:00
self.server.estimator.last_event = None
time.sleep(1.0 / self.server.max_measurements_per_second)
elif 'corners' in data.decode():
# send positions of the board markers
corner_estimates = self.server.estimator.corner_estimates
response = {}
for corner, data in corner_estimates.items():
response[corner] = {'x': data['x'], 'y': data['y']}
self.request.sendall((json.dumps(response) + '\n').encode())
2021-09-06 22:23:36 +00:00
elif 'move_grid' in data.decode():
data_decode = data.decode()
print("data: ", data_decode)
payload = data.decode().split(';')[1]
grid_pos = json.loads(payload)
print("grid_pos = ", grid_pos)
pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['orientation'])
self.server.estimator.event_queue.put(('click', {'x': pos[0], 'y': pos[1], 'angle': pos[2]}))
else:
# send robot position
try:
2020-11-17 20:23:22 +00:00
marker_id = int(data)
except ValueError:
marker_id = None
if marker_id is not None:
if marker_id in self.server.estimator.robot_marker_estimates:
while True:
2020-11-17 20:23:22 +00:00
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:
2020-11-17 20:23:22 +00:00
self.request.sendall("error: data not understood. "
"expected <robot marker id (int)> or 'events'\n".encode())
return
2020-10-24 19:16:17 +00:00
class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
allow_reuse_address = True
2020-11-17 20:23:22 +00:00
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")
2020-10-24 19:16:17 +00:00
2020-11-17 20:23:22 +00:00
if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[12, 13])
# first we start thread for the measurement server
measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, max_measurements_per_second=30)
server_thread = threading.Thread(target=measurement_server.serve_forever)
server_thread.start()
# now we start the Aruco estimator GUI
aruco_estimator.process_frame()
import sys
from pyqtgraph.Qt import QtCore, QtGui
if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
QtGui.QApplication.instance().exec_()
#with MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator,
# max_measurements_per_second=30) as measurement_server:
# measurement_server.serve_forever()
# receive with: nc 127.0.0.1 42424 -> 12 + Enter