diff --git a/remote_control/roborally.py b/remote_control/roborally.py index 6ada427..006785e 100644 --- a/remote_control/roborally.py +++ b/remote_control/roborally.py @@ -5,8 +5,7 @@ import time from mpc_controller import MPCController from robot import Robot - -import opencv_viewer_example +from aruco_estimator import ArucoEstimator MSGLEN = 64 @@ -75,7 +74,7 @@ class RoboRallyRobot(Robot): pass # nop command -> robot grid position does not change (used e.g. for driving the robot to initial # position) else: - print("error: invalid move") + print(f"error: invalid move: {move_type}") sys.exit(1) def __str__(self): @@ -91,7 +90,7 @@ class RemoteController: def __init__(self): self.robots = [] - self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')] + #self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')] self.robot_ids = {} for r in self.robots: @@ -110,7 +109,7 @@ class RemoteController: self.t = time.time() # start thread for marker position detection - self.estimator = opencv_viewer_example.ArucoEstimator(self.robot_ids.keys()) + self.estimator = ArucoEstimator(self.robot_ids.keys()) self.estimator_thread = threading.Thread(target=self.estimator.run_tracking) self.estimator_thread.start() @@ -171,7 +170,57 @@ class RemoteController: print("could not read robot id!") clientsocket.sendall(b'Could not read robot id!\n') - if robot_id in self.robot_ids: + if cmd == 'initialize_robot': + # add a new robot to the game + try: + id = int(inputs[1]) + ip = inputs[2].decode().strip() + x = int(inputs[3]) + y = int(inputs[4]) + orientation = inputs[5].decode().strip() + + print(f"initializing new robot with id {id} and ip {ip} at pos ({x},{y}) with " + f"orientation '{orientation}'") + if id not in self.robot_ids: + new_robot = RoboRallyRobot(id=id, ip=ip, x=x, y=y, orientation=orientation) + new_robot.connect() + + # store the new robot in the list of robots + self.robots.append(new_robot) + self.robot_ids[new_robot.id] = new_robot # this also means the estimator + # will track the new robot because + # it got a reference to the list of + # robot ids to keep an eye out for + + else: # robot is already created -> just re-initialize its position + new_robot = self.robot_ids[id] + new_robot.grid_x = x + new_robot.grid_y = y + new_robot.orientation = orientation + + if new_robot.connected: + print("created new robot and successfully connected to it!") + + while not self.estimator.all_robots_detected(): # wait until the robot gets detected + pass + + # drive the new robot to its starting position + self.grid_control(new_robot.id, 'nop') + + clientsocket.sendall(b'OK\n') + else: + clientsocket.sendall(f"error: could not connect to new robot {new_robot}".encode()) + + except IndexError: + print("could not initialize a new robot") + clientsocket.sendall('could not initialize a new robot: invalid command format\n' + 'expected: initialize_robot, , , , , \n'.encode()) + except ValueError: + print("could not initialize a new robot") + clientsocket.sendall('could not initialize a new robot: invalid command format\n' + 'expected: initialize_robot, , , , , \n'.encode()) + + elif robot_id in self.robot_ids: if cmd == b'get position': clientsocket.sendall(bytes('{}\n'.format(self.robot_ids[robot_id].grid_pos))) elif cmd == b'set position': @@ -194,46 +243,7 @@ class RemoteController: else: self.grid_control(robot_id, cmd) clientsocket.sendall(b'OK\n') - elif cmd == 'initialize_robot': - # add a new robot to the game - try: - id = int(inputs[1]) - ip = inputs[2].decode().strip() - x = int(inputs[3]) - y = int(inputs[4]) - orientation = inputs[5].decode().strip() - print(f"initializing new robot with id {id} and ip {ip} at pos ({x},{y}) with " - f"orientation '{orientation}'") - new_robot = RoboRallyRobot(id=id, ip=ip, x=x, y=y, orientation=orientation) - new_robot.connect() - if new_robot.connected: - print("created new robot and successfully connected to it!") - # store the new robot in the list of robots - self.robots.append(new_robot) - self.robot_ids[new_robot.id] = new_robot # this also means the estimator - # will track the new robot because - # it got a reference to the list of - # robot ids to keep an eye out for - - while not self.estimator.all_robots_detected(): # wait until the robot gets detected - pass - - # drive the new robot to its starting position - self.grid_control(new_robot.id, 'nop') - - clientsocket.sendall(b'OK\n') - else: - clientsocket.sendall(f"error: could not connect to new robot {new_robot}".encode()) - - except IndexError: - print("could not initialize a new robot") - clientsocket.sendall('could not initialize a new robot: invalid command format\n' - 'expected: initialize_robot, , , , , \n'.encode()) - except ValueError: - print("could not initialize a new robot") - clientsocket.sendall('could not initialize a new robot: invalid command format\n' - 'expected: initialize_robot, , , , , \n'.encode()) else: print("invalid robot id!") clientsocket.sendall(b'Invalid robot id!\n')