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

cleaner exit in case of errors

Browse Source
This commit is contained in:
Simon Pirkelmann 2021-09-09 21:34:54 +02:00
parent 019c4590aa
commit f3babdcf0a
1 changed files with 51 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
remote_control/measurement_server.py
View File

@ -12,12 +12,15 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
data = self.request.recv(1024).strip() data = self.request.recv(1024).strip()
cur_thread = threading.current_thread() cur_thread = threading.current_thread()
print(f"current thread {cur_thread}") print(f"start current thread {cur_thread}")
if 'events' in data.decode(): if 'events' in data.decode():
print(f"{cur_thread} subscribed to events")
self.request.sendall('subscribed to events\n'.encode()) self.request.sendall('subscribed to events\n'.encode())
# send any events in the event queue to the subscriber # send any events in the event queue to the subscriber
while True: event_loop_running = True
while event_loop_running:
try:
while not self.server.estimator.event_queue.empty(): while not self.server.estimator.event_queue.empty():
event = self.server.estimator.event_queue.get() event = self.server.estimator.event_queue.get()
# we distinguish two kinds of events: # we distinguish two kinds of events:
@ -35,7 +38,12 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
self.request.sendall((json.dumps(event) + '\n').encode()) self.request.sendall((json.dumps(event) + '\n').encode())
self.server.estimator.last_event = None self.server.estimator.last_event = None
time.sleep(1.0 / self.server.max_measurements_per_second) 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(): elif 'corners' in data.decode():
print(f"{cur_thread} subscribed to corners")
# send positions of the board markers # send positions of the board markers
corner_estimates = self.server.estimator.corner_estimates corner_estimates = self.server.estimator.corner_estimates
response = {} response = {}
@ -43,19 +51,23 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
response[corner] = {'x': data['x'], 'y': data['y']} response[corner] = {'x': data['x'], 'y': data['y']}
self.request.sendall((json.dumps(response) + '\n').encode()) self.request.sendall((json.dumps(response) + '\n').encode())
elif 'move_grid_blocking' in data.decode(): elif 'move_grid_blocking' in data.decode():
print(f"{cur_thread} subscribed move_grid_blocking")
# if we receive a move_grid event # if we receive a move_grid event
# ( e.g. move_grid;{"x":1,"y":1,"dimx":10,"dimy":10,"orientation":"^","require_response":True} ) # ( 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 # 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 # 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() data_decode = data.decode()
# print("data: ", data_decode) #print("data: ", data_decode)
payload = data.decode().split(';')[1] payload = data_decode.split(';')[1]
#print("payload: ", payload)
grid_pos = json.loads(payload) grid_pos = json.loads(payload)
# print("grid_pos = ", grid_pos) #print("grid_pos = ", grid_pos)
pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['dimx'], pos = self.server.estimator.get_pos_from_grid_point(grid_pos['x'], grid_pos['y'], grid_pos['dimx'],
grid_pos['dimy'], grid_pos['orientation']) grid_pos['dimy'], grid_pos['orientation'])
# print("pos = ", pos) #print("pos = ", pos)
# print("event requiring response") #print("event requiring response")
# put blocking move command in event queue # put blocking move command in event queue
self.server.estimator.event_queue.put(('response_event', self.server.estimator.event_queue.put(('response_event',
{'event': ('move_blocking', {'x': pos[0], 'y': pos[1], 'angle': pos[2]})})) {'event': ('move_blocking', {'x': pos[0], 'y': pos[1], 'angle': pos[2]})}))
@ -64,13 +76,15 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
# TODO this assumes that we wait only for at most one response at a time # 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) # 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(): while self.server.response_queue.empty():
time.sleep(0.1)
pass pass
reply = self.server.response_queue.get() reply = self.server.response_queue.get()
# send back response to the original source # send back response to the original source
#print(f"sending reply {reply} back to correspondent {self.request}") print(f"sending reply {reply} back to correspondent {self.request}")
self.request.sendall(reply) self.request.sendall(reply)
else: else:
print(f"{cur_thread} subscribed to robot position")
# send robot position # send robot position
try: try:
marker_id = int(data) marker_id = int(data)
@ -79,15 +93,22 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
if marker_id is not None: if marker_id is not None:
if marker_id in self.server.estimator.robot_marker_estimates: if marker_id in self.server.estimator.robot_marker_estimates:
while True: marker_loop_running = True
while marker_loop_running:
try:
self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id]) self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id])
+ '\n').encode()) + '\n').encode())
time.sleep(1.0 / self.server.max_measurements_per_second) 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: else:
self.request.sendall("error: unknown robot marker id\n".encode()) self.request.sendall("error: unknown robot marker id\n".encode())
else: else:
self.request.sendall("error: data not understood. " self.request.sendall("error: data not understood. "
"expected <robot marker id (int)> or 'events'\n".encode()) "expected <robot marker id (int)> or 'events'\n".encode())
print(f"end current thread {cur_thread}")
return return
@ -105,7 +126,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
if __name__ == "__main__": if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=False, robot_marker_ids=[12, 13]) aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[12, 13])
# first we start thread for the measurement server # first we start thread for the measurement server
measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator, measurement_server = MeasurementServer(('0.0.0.0', 42424), MeasurementHandler, aruco_estimator,