RoboRally/remote_control/roborally.py

import sys
import socket
import threading
import time

from mpc_controller import MPCController
from robot import Robot
from aruco_estimator import ArucoEstimator

MSGLEN = 64

def myreceive(sock):
    chunks = []
    bytes_recd = 0
    while bytes_recd < MSGLEN:
        chunk = sock.recv(1)
        if chunk == b'':
            raise RuntimeError("socket connection broken")
        chunks.append(chunk)
        bytes_recd = bytes_recd + len(chunk)

        if chunks[-1] == b'\n':
            break

    return b''.join(chunks)


class RoboRallyRobot(Robot):
    # dictionary mapping the current orientation and a turn command to the resulting orientation
    resulting_orientation = {
        '^': {'turn left': '<', 'turn right': '>', 'turn around': 'v'},
        '>': {'turn left': '^', 'turn right': 'v', 'turn around': '<'},
        'v': {'turn left': '>', 'turn right': '<', 'turn around': '^'},
        '<': {'turn left': 'v', 'turn right': '^', 'turn around': '>'},
    }

    # dictionary mapping an orientation to its opposite
    opposites = {'^': 'v', '>': '<', 'v': '^', '<': '>'}

    def __init__(self, id, ip, x, y, orientation):
        super().__init__(id, ip)

        self.grid_x = x
        self.grid_y = y
        self.grid_orientation = orientation

    def get_neighbor_coordinates(self, direction):
        # get the coordinates of the neighboring tile in the given direction
        if direction == '^':
            return self.grid_x, self.grid_y - 1
        elif direction == '>':
            return self.grid_x + 1, self.grid_y
        elif direction == 'v':
            return self.grid_x, self.grid_y + 1
        elif direction == '<':
            return self.grid_x - 1, self.grid_y
        else:
            print("error: unknown direction")
            sys.exit(1)

    def move(self, move_type):
        if move_type == 'forward':
            target_tile = self.get_neighbor_coordinates(self.grid_orientation)
            self.grid_x = target_tile[0]
            self.grid_y = target_tile[1]
        elif move_type == 'backward':
            opposite_orientation = RoboRallyRobot.opposites[self.grid_orientation]
            target_tile = self.get_neighbor_coordinates(opposite_orientation)
            self.grid_x = target_tile[0]
            self.grid_y = target_tile[1]
        elif 'turn' in move_type:
            self.grid_orientation = RoboRallyRobot.resulting_orientation[self.grid_orientation][move_type]
        elif 'nop' in move_type:
            pass    # nop command -> robot grid position does not change (used e.g. for driving the robot to initial
                    #                                                      position)
        else:
            print(f"error: invalid move: {move_type}")
            sys.exit(1)

    def __str__(self):
        return self.__repr__()

    def __repr__(self):
        return f"grid x: {self.grid_x}, grid y: {self.grid_y}, grid orientation: {self.grid_orientation}"


class RemoteController:
    valid_cmds = ['forward', 'backward', 'turn left', 'turn right', 'turn around', 'nop', 'get position',
                  'set position', 'initialize_robot', 'initialize_grid']

    def __init__(self):
        self.robots = []
        #self.robots = [RoboRallyRobot(12, '192.168.1.12', x=1, y=1, orientation='>')]

        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 = ArucoEstimator(self.robot_ids.keys())
        self.estimator_thread = threading.Thread(target=self.estimator.run_tracking)
        self.estimator_thread.start()

        self.controller = MPCController(self.estimator)

        print("waiting for corner and robot detection..")
        while not self.estimator.all_robots_detected() or not self.estimator.all_corners_detected():
            pass
        print("everything detected!")
        # drive robots to initial position
        for robot_id in self.robot_ids:
            self.grid_control(robot_id, 'nop')

    def run(self):
        print("waiting until all markers are detected...")
        while not self.estimator.all_corners_detected():
            pass
        print("starting control")

        running = True
        while running:
            (clientsocket, address) = self.comm_socket.accept()
            clientsocket.settimeout(None)

            connected = True
            while connected:
                try:
                    data = myreceive(clientsocket)
                    print("data received: ", data)

                    inputs = data.split(b',')
                    cmd = inputs[0]
                    cmd = cmd.strip().decode()

                    if len(inputs) > 1:
                        if cmd in RemoteController.valid_cmds:
                            if cmd == 'initialize_grid':
                                try:
                                    grid_columns = int(inputs[1])
                                    grid_rows = int(inputs[2])

                                    self.estimator.grid_columns = grid_columns
                                    self.estimator.grid_rows = grid_rows
                                    clientsocket.sendall(b'OK\n')
                                except ValueError:
                                    print("could not initialize grid!")
                                    clientsocket.sendall(b'"could not initialize grid!\n'
                                                         b'expected: initialize_robot, <grid columns>, <grid rows>')
                                except IndexError:
                                    print("could not initialize grid!")
                                    clientsocket.sendall(b'"could not initialize grid!\n'
                                                         b'expected: initialize_robot, <grid columns>, <grid rows>')
                            else:   # robot command
                                try:
                                    robot_id = int(inputs[1])
                                except ValueError:
                                    robot_id = None
                                    print("could not read robot id!")
                                    clientsocket.sendall(b'Could not read robot id!\n')

                                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, <id>, <ip>, <x>, <y>, <orientation>\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, <id>, <ip>, <x>, <y>, <orientation>\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':
                                        try:
                                            pos_data = ",".join(inputs[2:])
                                            new_grid_pos = tuple(map(lambda x: int(x[1]) if x[0] < 2 else float(x[1]),
                                                                     enumerate(pos_data.strip().strip('()').split(','))))
                                            self.robot_ids[robot_id].grid_pos = new_grid_pos
                                            clientsocket.sendall(b'OK\n')
                                        except IndexError as e:
                                            print("could not set grid position!")
                                            clientsocket.sendall(bytes(
                                                'could not set grid position! (invalid format)\n'.format(
                                                    self.robot_ids[robot_id].grid_pos)))
                                        except ValueError as e:
                                            print("could not set grid position!")
                                            clientsocket.sendall(bytes(
                                                'could not set grid position! (invalid format)\n'.format(
                                                    self.robot_ids[robot_id].grid_pos)))
                                    else:
                                        self.grid_control(robot_id, cmd)
                                        clientsocket.sendall(b'OK\n')

                                else:
                                    print("invalid robot id!")
                                    clientsocket.sendall(b'Invalid robot id!\n')
                        else:
                            clientsocket.sendall(b'Invalid command!\n')
                    else:  # len(inputs) <= 1
                        if b'quit' in inputs[0]:
                            clientsocket.close()
                            self.comm_socket.close()
                            connected = False
                            running = False
                        else:
                            print("could not process command!")
                            clientsocket.sendall(b'Could not process request!\n')

                except RuntimeError:
                    print("disconnected")
                    connected = False
                    clientsocket.close()

    def grid_control(self, robot_id, cmd):
        robot = self.robot_ids[robot_id]  # get robot to be controlled

        print("robot grid pos before move: ", robot)
        robot.move(cmd)
        print("robot grid pos after move: ", robot)

        target = self.estimator.get_pos_from_grid_point(robot.grid_x, robot.grid_y, robot.grid_orientation)

        self.controller.move_to_pos(target, robot)


def main(args):
    rc = RemoteController()
    rc.run()


if __name__ == '__main__':
    main(sys.argv)