forked from Telos4/RoboRally
84 lines
3.0 KiB
Python
84 lines
3.0 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 CommanderBase:
|
|
def __init__(self):
|
|
self.robots = []
|
|
self.robots = [ControlledRobot(12, '10.10.11.91'), ControlledRobot(13, '10.10.11.90')] #, Robot(14, '192.168.1.14')]
|
|
# self.robots = [ControlledRobot(marker_id=13, ip='10.10.11.90'),
|
|
# ControlledRobot(marker_id=14, ip='10.10.11.89')]
|
|
|
|
for r in self.robots:
|
|
r.connect()
|
|
|
|
self.robots[0].attach_controller(PIDController())
|
|
self.robots[1].attach_controller(MPCController())
|
|
|
|
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))
|
|
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))
|
|
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':
|
|
target = event[1]
|
|
target_pos = np.array([target['x'], target['y'], target['angle']])
|
|
self.robots[self.current_robot_index].move_to_pos(target_pos)
|
|
elif event[0] == 'key':
|
|
key = event[1]
|
|
if key == 32: # arrow up
|
|
self.controlling = not self.controlling
|
|
if not self.controlling:
|
|
print("disable control")
|
|
for r in self.robots:
|
|
r.stop_control()
|
|
else:
|
|
print("enable control")
|
|
for r in self.robots:
|
|
r.start_control()
|
|
elif key == 9: # TAB
|
|
# switch controlled robot
|
|
self.current_robot_index = (self.current_robot_index + 1) % len(self.robots)
|
|
robot = self.robots[self.current_robot_index]
|
|
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__':
|
|
rc = CommanderBase()
|
|
rc.run()
|