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

implemented blocking events to make it possible to wait for the robot to reach the desired target position

Browse Source
This commit is contained in:
Simon Pirkelmann 2021-09-07 22:31:28 +02:00
parent 2c54e56f95
commit 3082eebc8d
3 changed files with 66 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
remote_control/control_commander.py
View File

@ -7,7 +7,7 @@ from mpc_controller import MPCController
from event_listener import EventListener
class CommanderBase:
class ControlCommander:
valid_controller_types = {'pid': PIDController,
'mpc': MPCController}
@ -26,17 +26,17 @@ class CommanderBase:
if type(controller_type) == dict:
for id, ctype in controller_type.items():
if ctype in CommanderBase.valid_controller_types:
self.robots[id].attach_controller(CommanderBase.valid_controller_types[ctype]())
if ctype in ControlCommander.valid_controller_types:
self.robots[id].attach_controller(ControlCommander.valid_controller_types[ctype]())
else:
raise Exception(f"invalid controller type {ctype} specified for robot {id}. "
f"valid controller types are {list(CommanderBase.valid_controller_types.keys())}")
elif controller_type in CommanderBase.valid_controller_types:
f"valid controller types are {list(ControlCommander.valid_controller_types.keys())}")
elif controller_type in ControlCommander.valid_controller_types:
for id, r in self.robots.items():
r.attach_controller(CommanderBase.valid_controller_types[controller_type]())
r.attach_controller(ControlCommander.valid_controller_types[controller_type]())
else:
raise Exception(f"invalid controller type {controller_type} specified. valid controller types are "
f"{list(CommanderBase.valid_controller_types.keys())}")
f"{list(ControlCommander.valid_controller_types.keys())}")
self.event_listener = EventListener(event_server=('127.0.0.1', 42424))
@ -68,11 +68,21 @@ class CommanderBase:
def handle_event(self, event):
# handle events from opencv window
print("event: ", event)
if event[0] == 'click':
if event[0] == 'click': # non-blocking move to target pos
target = event[1]
target_pos = np.array([target['x'], target['y'], target['angle']])
controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
self.robots[controlled_robot_id].move_to_pos(target_pos)
elif event[0] == 'move_blocking': # blocking move to specified target position
target = event[1]
target_pos = np.array([target['x'], target['y'], target['angle']])
controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
# initialize move and wait for the robot to reach the target position
self.robots[controlled_robot_id].move_to_pos(target_pos, blocking=True)
#time.sleep(0.5)
# send confirmation to the server which initiated the command
self.event_listener.send_reply("ack\n".encode())
elif event[0] == 'key':
key = event[1]
if key == 16777235: # arrow up
@ -109,5 +119,5 @@ if __name__ == '__main__':
# controller_type = {12: 'mpc', 13: 'pid'}
controller_type = 'pid'
rc = CommanderBase(id_ip_dict, controller_type=controller_type)
rc = ControlCommander(id_ip_dict, controller_type=controller_type)
rc.run()

3
remote_control/event_listener.py
View File

@ -34,6 +34,9 @@ class EventListener:
except ConnectionRefusedError:
print(f"error: could not connect to event server at {event_server}.")
def send_reply(self, data):
self.event_socket.sendall(data)
def receive_events(self):
self.receiving = True
while self.receiving:

53
remote_control/measurement_server.py
View File

@ -2,6 +2,7 @@ import socketserver
import threading
import time
import json
from queue import Queue
from aruco_estimator import ArucoEstimator
@ -15,11 +16,23 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
if 'events' in data.decode():
self.request.sendall('subscribed to events\n'.encode())
# send input events
# send any events in the event queue to the subscriber
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())
# 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)
elif 'corners' in data.decode():
@ -29,14 +42,34 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
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' in data.decode():
elif 'move_grid_blocking' in data.decode():
# if we receive a move_grid event
# ( e.g. move_grid;{"x":1,"y":1,"dimx":10,"dimy":10,"orientation":"^","require_response":True} )
# 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)
# 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]}))
# 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():
pass
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:
# send robot position
try:
@ -65,16 +98,18 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
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])
aruco_estimator = ArucoEstimator(use_realsense=False, 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)
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()