import sys import socket import threading import time from robot import Robot from mpc_controller import MPCController import opencv_viewer_example class RemoteController: def __init__(self): self.robots = [Robot(12, '192.168.1.12')] self.robot_ids = {} for r in self.robots: self.robot_ids[r.id] = r # socket for movement commands self.comm_socket = socket.socket() self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) for robot in self.robots: robot.connect() self.comm_socket.bind(('', 1337)) self.comm_socket.listen(5) self.t = time.time() # start thread for marker position detection self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys()) self.estimator_thread = threading.Thread(target=self.estimator.run_tracking) self.estimator_thread.start() self.controller = MPCController(self.estimator) def run(self): print("waiting until all markers are detected...") while not (self.estimator.all_corners_detected() and self.estimator.all_robots_detected()): pass print("starting control") self.controller.interactive_control(robots=self.robots) def main(args): rc = RemoteController() rc.run() if __name__ == '__main__': main(sys.argv)