124 lines
5.2 KiB
Python
124 lines
5.2 KiB
Python
import numpy as np
|
|
import time
|
|
|
|
from robot import ControlledRobot
|
|
from pid_controller import PIDController
|
|
from mpc_controller import MPCController
|
|
from event_listener import EventListener
|
|
|
|
|
|
class ControlCommander:
|
|
valid_controller_types = {'pid': PIDController,
|
|
'mpc': MPCController}
|
|
|
|
def __init__(self, id_ip_dict, controller_type='pid'):
|
|
"""
|
|
|
|
:param id_ip_dict: dictionary containing robot marker id and ip of the robot, e.g. { 12: '10.10.11.91' }
|
|
:param controller_type: string 'pid', 'mpc'; or dictionary with robot id and controller type string, e.g. { 12: 'mpc', 13: 'pid'}
|
|
"""
|
|
self.robots = {}
|
|
for id, ip in id_ip_dict.items():
|
|
self.robots[id] = ControlledRobot(id, ip)
|
|
|
|
for id, r in self.robots.items():
|
|
r.connect()
|
|
|
|
if type(controller_type) == dict:
|
|
for id, ctype in controller_type.items():
|
|
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(ControlCommander.valid_controller_types.keys())}")
|
|
elif controller_type in ControlCommander.valid_controller_types:
|
|
for id, r in self.robots.items():
|
|
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(ControlCommander.valid_controller_types.keys())}")
|
|
|
|
self.event_listener = EventListener(event_server=('127.0.0.1', 42424))
|
|
|
|
self.current_robot_index = 0
|
|
self.controlling = False
|
|
self.running = False
|
|
|
|
def run(self):
|
|
unconnected_robots = list(filter(lambda r: not r.connected, self.robots.values()))
|
|
if len(unconnected_robots) > 0:
|
|
print(f"warning: could not connect to the following robots: {unconnected_robots}")
|
|
return
|
|
|
|
all_detected = False
|
|
while not all_detected:
|
|
undetected_robots = list(filter(lambda r: None in r.get_measurement(), self.robots.values()))
|
|
all_detected = len(undetected_robots) == 0
|
|
if not all_detected:
|
|
print(f"warning: no measurements available for the following robots: {undetected_robots}")
|
|
time.sleep(0.5)
|
|
|
|
print("starting control")
|
|
self.running = True
|
|
while self.running:
|
|
while not self.event_listener.event_queue.empty():
|
|
event = self.event_listener.event_queue.get()
|
|
self.handle_event(event)
|
|
|
|
def handle_event(self, event):
|
|
# handle events from opencv window
|
|
print("event: ", event)
|
|
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
|
|
self.controlling = not self.controlling
|
|
if not self.controlling:
|
|
print("disable control")
|
|
for _, r in self.robots.items():
|
|
r.stop_control()
|
|
else:
|
|
print("enable control")
|
|
for _, r in self.robots.items():
|
|
r.start_control()
|
|
elif key == 16777236: # left
|
|
# switch controlled robot
|
|
self.current_robot_index = (self.current_robot_index + 1) % len(self.robots)
|
|
controlled_robot_id = list(self.robots.keys())[self.current_robot_index]
|
|
robot = self.robots[controlled_robot_id]
|
|
print(f"controlled robot: {robot.id}")
|
|
elif key == 113 or key == 27: # q or ESCAPE
|
|
print("quit!")
|
|
for r in self.robots:
|
|
r.stop_control()
|
|
self.running = False
|
|
|
|
|
|
if __name__ == '__main__':
|
|
id_ip_dict = {
|
|
#11: '10.10.11.88',
|
|
12: '192.168.1.12',
|
|
13: '192.168.1.13',
|
|
#14: '10.10.11.89',
|
|
}
|
|
|
|
# controller_type = {12: 'mpc', 13: 'pid'}
|
|
controller_type = 'pid'
|
|
|
|
rc = ControlCommander(id_ip_dict, controller_type=controller_type)
|
|
rc.run()
|