RoboRally/remote_control/mpc_test.py

56 lines
1.4 KiB
Python
Raw Normal View History

import sys
import socket
import threading
import time
from robot import Robot
from mpc_controller import MPCController
2020-10-24 17:59:56 +00:00
from aruco_estimator import ArucoEstimator
class RemoteController:
def __init__(self):
2020-10-24 17:59:56 +00:00
self.robots = []
#self.robots = [Robot(11, '192.168.1.11'), Robot(13, '192.168.1.13'), Robot(14, '192.168.1.14')]
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
2020-10-24 17:59:56 +00:00
self.estimator = 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)
2020-10-24 19:12:03 +00:00
def main(args):
rc = RemoteController()
rc.run()
if __name__ == '__main__':
main(sys.argv)