152 lines
7.3 KiB
Python
152 lines
7.3 KiB
Python
import socketserver
|
|
import threading
|
|
import time
|
|
import json
|
|
from queue import Queue
|
|
|
|
from aruco_estimator import ArucoEstimator
|
|
|
|
|
|
class MeasurementHandler(socketserver.BaseRequestHandler):
|
|
def handle(self) -> None:
|
|
data = self.request.recv(1024).strip()
|
|
|
|
cur_thread = threading.current_thread()
|
|
print(f"start current thread {cur_thread}")
|
|
|
|
if 'events' in data.decode():
|
|
print(f"{cur_thread} subscribed to events")
|
|
self.request.sendall('subscribed to events\n'.encode())
|
|
# send any events in the event queue to the subscriber
|
|
event_loop_running = True
|
|
while event_loop_running:
|
|
try:
|
|
while not self.server.estimator.event_queue.empty():
|
|
event = self.server.estimator.event_queue.get()
|
|
# we distinguish two kinds of events:
|
|
if event[0] == 'response_event':
|
|
# 1. for 'response_event' events we expect the event subscriber to give a reply which will then
|
|
# by passed on to the response queue for transmitting to the original correspondent
|
|
message = event[1]['event']
|
|
#print(f"passing command {message} on to subscriber")
|
|
self.request.sendall((json.dumps(message) + '\n').encode())
|
|
reply = self.request.recv(1024)
|
|
#print(f"putting reply {reply} in response queue")
|
|
self.server.response_queue.put(reply)
|
|
else:
|
|
# 2. for other types of events we don't expect a reply and just pass them on to the subscriber
|
|
self.request.sendall((json.dumps(event) + '\n').encode())
|
|
self.server.estimator.last_event = None
|
|
time.sleep(1.0 / self.server.max_measurements_per_second)
|
|
except Exception as e:
|
|
print(f"exception in event loop: {e}")
|
|
event_loop_running = False
|
|
print("after event loop")
|
|
elif 'corners' in data.decode():
|
|
print(f"{cur_thread} subscribed to corners")
|
|
# 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())
|
|
elif 'move_grid_blocking' in data.decode():
|
|
print(f"{cur_thread} subscribed move_grid_blocking")
|
|
|
|
# if we receive a move_grid event
|
|
# ( e.g. move_grid_blocking;{"x":1,"y":1,"dimx":10,"dimy":10,"orientation":"^"} )
|
|
#move_grid_blocking;{"x": 1, "y": 2, "dimx": 7, "dimy": 4, "orientation": ">"}
|
|
# we compute the corresponding real-world position the robot should drive to
|
|
# and then create a new move event which is put in the event queue and will be propagated to the ControlCommander
|
|
data_decode = data.decode()
|
|
#print("data: ", data_decode)
|
|
payload = data_decode.split(';')[1]
|
|
#print("payload: ", payload)
|
|
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['dimx'],
|
|
grid_pos['dimy'], grid_pos['orientation'])
|
|
#print("pos = ", pos)
|
|
#print("event requiring response")
|
|
# put blocking move command in event queue
|
|
self.server.estimator.event_queue.put(('response_event',
|
|
{'event': ('move_blocking', {'x': pos[0], 'y': pos[1], 'angle': pos[2]})}))
|
|
|
|
# wait for response of the move command
|
|
# TODO this assumes that we wait only for at most one response at a time
|
|
# we could add some kind of reference here to handle multiple responses (e.g. id of the response to wait for)
|
|
while self.server.response_queue.empty():
|
|
time.sleep(0.2)
|
|
self.request.sendall(b'.\n')
|
|
pass
|
|
while not self.server.response_queue.empty():
|
|
reply = self.server.response_queue.get()
|
|
|
|
# send back response to the original source
|
|
print(f"sending reply {reply} back to correspondent {self.request}")
|
|
self.request.sendall(reply)
|
|
else:
|
|
print(f"{cur_thread} subscribed to robot position")
|
|
# send robot position
|
|
try:
|
|
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:
|
|
marker_loop_running = True
|
|
while marker_loop_running:
|
|
try:
|
|
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)
|
|
except Exception as e:
|
|
print(f"exception in marker loop: {e}")
|
|
marker_loop_running = False
|
|
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())
|
|
|
|
print(f"end current thread {cur_thread}")
|
|
return
|
|
|
|
|
|
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
|
|
self.response_queue = Queue()
|
|
|
|
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=[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
|