diff --git a/remote_control/measurement_server.py b/remote_control/measurement_server.py index d9942b9..4a4cbcf 100644 --- a/remote_control/measurement_server.py +++ b/remote_control/measurement_server.py @@ -22,7 +22,13 @@ class MeasurementHandler(socketserver.BaseRequestHandler): self.request.sendall((json.dumps(event) + '\n').encode()) 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()) else: # send robot position try: @@ -57,12 +63,23 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer): if __name__ == "__main__": - aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14]) - estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True}) - estimator_thread.start() + aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[12, 13]) - with MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, - max_measurements_per_second=30) as measurement_server: - measurement_server.serve_forever() + # 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