RoboRally/remote_control/control_commander.py

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()