import socket import sys import threading import numpy as np from socket import error as SocketError class Robot(): # frente # --------- # | | # | | # | LLL | # | LLL | # --------- # idd, id do robo # xsize, largura do robo # xsize, comprimento do robo # xtag, distancia horizontal do lado esquerdo ao cento do tag # ytag, distancia vertical do topo ao cento do tag # posx, posy, course, posicao do robo def __init__(self, idd, xsize, ysize, xtag, ytag, posx = 0, posy = 0, course = 0): self.idd = idd self.xsize = xsize self.ysize = ysize self.xtag = xtag self.ytag = ytag self.posx = posx self.posy = posy self.course = course def lines(self): p1 = np.array([-self.xtag, self.ytag]) p2 = np.array([-self.xtag, -self.ysize+self.ytag]) p3 = np.array([self.xsize-self.xtag, self.ytag]) p4 = np.array([self.xsize-self.xtag, -self.ysize+self.ytag]) pos = np.array([self.posx, self.posy]) c, s = np.cos(self.course), np.sin(self.course) R = np.array(((c,-s), (s, c))) p1 = p1.dot(R)+pos p2 = p2.dot(R)+pos p3 = p3.dot(R)+pos p4 = p4.dot(R)+pos return np.array([[p1, p2], [p2, p4], [p4, p3], [p1, p3]]) def ccw(self, A,B,C): return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0]) # Return true if line segments AB and CD intersect def line_intersect(self, A,B,C,D): return ccw(A,C,D) != ccw(B,C,D) and ccw(A,B,C) != ccw(A,B,D) # get robot and see if this intersect te other def intersect(self, robot): thislines = self.lines() other = robot.lines() for i in thislines: for j in thislines: if line_intersect(i[0], i[1], j[0], j[1]): return True return False class TagServer(threading.Thread): def __init__(self, port, board_size): threading.Thread.__init__(self) sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server_address = ('0.0.0.0', port) print ('Starting up on %s port %s' % server_address) sock.bind(server_address) sock.listen(1) sock.settimeout(1) self.sock = sock self.clients_threads = [] self.tag = None self.board_size = board_size self.runV = True self.start() def updatePosition (self, idd, posx, posy, course): for thread in self.clients_threads: if thread.robot is not None and thread.robot.idd == idd: msg = "P " msg = msg + str(posx)+"," msg = msg + str(posy)+"," msg = msg + str(course)+"," msg = msg + str(thread.robot.idd)+"," msg = msg + str(thread.robot.xsize)+"," msg = msg + str(thread.robot.ysize)+"," msg = msg + str(thread.robot.xtag)+"," msg = msg + str(thread.robot.ytag) thread.robot.posx = posx thread.robot.posy = posy thread.robot.course = course self.brodcast(msg) # check for collision for i in self.robots(): if i != self.tag: continue for j in self.robots(): if i == j: continue if i.intersect(j): self.tag = j self.brodcast("RUN "+str(self.tag.id)+" 10") def startGame (self): self.brodcast("STOP") def startGame (self): self.brodcast("START") if self.tag is None: tag = np.random.choice(self.robots(), 1) if len(tag) == 1: self.tag = tag self.brodcast("RUN "+str(self.tag.id)+" 10") return True return False def brodcast(self, msg): for thread in self.clients_threads: if thread.robot is not None: thread.send(msg+"\n") def run(self): while self.runV: try: (clientsock, (ip, port)) = self.sock.accept() newthread = TagThread(ip, port, clientsock, self) self.clients_threads.append(newthread) except socket.timeout as err: pass self.sock.close() def robots(self): r = [] for thread in self.clients_threads: if thread.robot is not None: r.append(thread.robot) return r def stop(self): for thread in self.clients_threads: thread.stop() self.runV = False class TagThread(threading.Thread): def __init__(self, ip, port, socket, server): threading.Thread.__init__(self) self.ip = ip self.port = port self.sock = socket self.points = 0 self.robot = None self.bz = server.board_size self.server = server socket.settimeout(1) self.runV = True self.start() def run(self): while self.runV: try: data = self.sock.recv(64).decode('ascii') except socket.timeout as err: continue except SocketError as e: self.robot = None break data = data.split(" ") try: if data[0] == "SUB": idd, xsize, ysize, xtag, ytag = data[1].split(",") self.robot = Robot(int(idd), float(xsize), float(ysize), float(xtag), float(ytag)) self.send("OK "+str(self.bz[0])+","+str(self.bz[1])+"\n") if data[0] == "P": posx, posy, curse = data[1].split(",") posx, posy, curse = float(posx), float(posy), float(curse) self.server.updatePosition(self.robot.idd, posx, posy, curse) except Exception as e: print(e) self.send("ERR\n") self.sock.close() def send(self, msg): self.sock.sendall(msg.encode('ascii')) def stop(self): self.runV = False class TagClient(threading.Thread): def __init__(sself, ip, port): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server_address = (ip, port) sock.connect(server_address) self.sock = sock self.stop = None self.start = None self.tagger = None self.position = None def info (self, idd, xsize, ysize, xtag, ytag): message = "SUB "+idd+","+xsize+","+ysize+","+xtag+","+ytag+"\n" self.sock.sendall(message.encode('ascii')) self.start() def run(self): while True: try: data = self.sock.recv(64).decode('ascii') except SocketError as e: self.robot = None break data = data.split("\n")[0].split(" ") if data[0] == "OK": self.board_size = self.position(data[1].split(",")) print("SUB OK") elif data[0] == "P" and self.position is not None: self.position(data[1].split(",")) def stopCallback (self, function): self.stop = function def startCallback (self, function): self.start = function def taggerCallback (self, function): self.tagger = function def positionCallback (self, function): self.position = function def boardinfoCallback (self, function): self.boardinfo = function # Somente na simulacao def setPosition(px, py): message = "P "+px+","+py+"\n" self.sock.sendall(message.encode('ascii'))