Adicionando servidor e clientes

.gitignore

@@ -0,0 +1,2 @@
+__pycache__
+__pycache__/

cmarkers.py

@@ -5,8 +5,8 @@ from numpy.linalg import norm
 
 def getCMarkers (img):
     markers = []
-    img3 = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
-    img3 = cv2.adaptiveThreshold(img3, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 125, 10)
+    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+    img3 = cv2.adaptiveThreshold(gray, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 125, 10)
 
     kernel = np.ones((6,6),np.uint8)
     img2 = cv2.morphologyEx(img3, cv2.MORPH_CLOSE, kernel)
@@ -70,7 +70,7 @@ def getCMarkers (img):
     for point in k:
         p, near = point
         # distance open the angre and 90 degree
-        midistance = math.pi/15.0
+        midistance = math.pi/12.5
         bottom = [] 
         rigth = [] 
         for p1 in near:
@@ -86,7 +86,7 @@ def getCMarkers (img):
                     rigth = p2
                     midistance = abs(angle-math.pi/2.0)
 
-        if midistance == math.pi/15.0:
+        if midistance == math.pi/12.5:
             continue
 
         u = np.array([bottom[0]-p[0], bottom[1]-p[1]])
@@ -96,8 +96,8 @@ def getCMarkers (img):
         addv = v*1.0/6.0
         conners = [p-addu-addv, bottom+addu-addv, rigth-addu+addv, conner+addu+addv]
         trans = get_transform_matrix_points(conners, [10, 10], 10)
-        code = cv2.warpPerspective(img2, trans, dsize=(100, 100))
-        code = cv2.erode(code, kernel,iterations = 1)
+        code = cv2.warpPerspective(gray, trans, dsize=(100, 100))
+        # code = cv2.erode(code, kernel,iterations = 1)
         # cv2.imshow("m2", code)
 
         vsplit = np.vsplit(code, 4)
@@ -108,7 +108,8 @@ def getCMarkers (img):
             for hs in hsplit:
                 m.append(np.mean(hs))
             mean.append(m)
-        mean = np.array(mean) > 210.0
+        # print(np.array(mean).astype(np.uint8))
+        mean = np.array(mean) >= 100.0
         valid = mean[0, 0] == False
         valid = valid and mean[0, 3] == False
         valid = valid and mean[0, 3] == False

gem-cmarkers.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+import sys
+from cv2 import imwrite
+
+
+from numpy import mean, binary_repr, ones
+from numpy.random import randint
+from scipy.ndimage import zoom
+
+for i in range(0, 16):
+    img = ones((6, 6))*255
+    img[1, 1] = 0
+    img[4, 1] = 0
+    img[1, 4] = 0
+
+    if i%2 == 1:
+        img[2, 2] = 0
+
+    if (i>>1)%2 == 1:
+        img[2, 3] = 0
+
+    if (i>>2)%2 == 1:
+        img[3, 2] = 0
+
+    if (i>>3)%2 == 1:
+        img[3, 3] = 0
+
+    print (img)
+    marker = zoom(img, zoom=50, order=0)
+
+    imwrite('marker_images/marker_{}.png'.format(i), marker)

player-camera.py

@@ -0,0 +1,74 @@
+# Simula virtualmente um robo
+from robot import Robot
+from tag import TagClient
+from random import randint, random
+import signal, sys
+import math
+import time
+
+rid = 14
+run = False
+board = None
+tagplayer = -1
+
+client = TagClient('localhost', 10318)
+client.info(rid, 8, 8, 4, 4)
+
+px, py, course = None, None, None
+
+# Sinal de ctrl-c
+def signal_handler(signal, frame):
+    client.stop()
+    sys.exit(0)
+signal.signal(signal.SIGINT, signal_handler)
+
+players = {}
+
+while True:
+    time.sleep(0.1)
+
+    # nao recebeu informacoes do campo
+    if client.getBoard() is None:
+        continue
+    elif px is None:
+        board = client.getBoard()
+        px, py = board[0]*random(), board[1]*random()
+        course = math.pi*2.0*random()
+
+    # jogo esta pausado
+    if client.getStatus() is False:
+        players = {}
+        continue
+
+    # nao recebeu informacoes do pegador
+    if client.getTagInfo() is None:
+        continue
+
+    # verifica se ha posicoes novas
+    newposs = client.getPosition()
+    for info in newposs:
+        posx, posy, course, idd, xsize, ysize, xtag, ytag = info
+        if idd != rid:
+            players[idd] = [posx, posy]
+        else:
+            # atualiza posicao
+            px, py, course = posx, posy, course
+
+    tag, interval = client.getTagInfo()  
+    pos = [players[p] for p in players]
+
+    if time.time() <= interval:
+        # comportamento de fuga
+        if tag in players and tag != rid:
+            # foge se nao for pegador 
+            pass
+    else:
+
+        if len(pos) >= 1 and tag == rid:
+            # acoes caso seja o pegador
+            pass
+
+        if tag != rid:
+            #acoes de fuga
+            pass
+

player-lejos.py

@@ -0,0 +1,18 @@
+import bluetooth
+# pybluez
+nearby_devices = bluetooth.discover_devices(lookup_names=True)
+print("found %d devices" % len(nearby_devices))
+
+for addr, name in nearby_devices:
+    print("  %s - %s" % (addr, name))
+
+serverMACAddress = '00:16:53:17:ea:9d'
+port = 3
+s = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
+s.connect((serverMACAddress, port))
+while 1:
+    text = raw_input() # Note change to the old (Python 2) raw_input
+    if text == "quit":
+        break
+    s.send(text)
+sock.close()

player-virtual.py

@@ -0,0 +1,82 @@
+# Simula virtualmente um robo
+from robot import Robot
+from tag import TagClient
+from random import randint, random
+import signal, sys
+import math
+import time
+from numpy.linalg import norm
+import numpy as np
+
+rid = randint(100, 1000)
+run = False
+board = None
+tagplayer = -1
+
+client = TagClient('localhost', 10318)
+client.info(rid, 8, 8, 4, 4)
+
+px, py, course = None, None, None
+
+# Sinal de ctrl-c
+def signal_handler(signal, frame):
+    client.stop()
+    sys.exit(0)
+signal.signal(signal.SIGINT, signal_handler)
+
+players = {}
+
+while True:
+    time.sleep(0.1)
+
+    # nao recebeu informacoes do campo
+    if client.getBoard() is None:
+        continue
+    elif px is None:
+        board = client.getBoard()
+        px, py = board[0]*random(), board[1]*random()
+        course = math.pi*2.0*random()
+        client.setPosition(px, py, course)
+
+    # jogo esta pausado
+    if client.getStatus() is False:
+        players = {}
+        continue
+
+    # nao recebeu informacoes do pegador
+    if client.getTagInfo() is None:
+        continue
+
+    # verifica se ha posicoes novas
+    newposs = client.getPosition()
+    for info in newposs:
+        posx, posy, course, idd, xsize, ysize, xtag, ytag = info
+        if idd != rid:
+            players[idd] = [posx, posy]
+
+    tag, interval = client.getTagInfo()  
+    pos = [players[p] for p in players]
+
+    if time.time() <= interval:
+        # comportamento de fuga
+        if tag in players and tag != rid:
+            # foge se nao for pegador 
+            px = px + (px-players[tag][0])/90.0
+            py = py + (py-players[tag][1])/90.0
+
+            course = course+(random()-0.5)/5.0
+            client.setPosition(px, py, course)
+    else:
+
+        if len(pos) >= 1 and tag == rid:
+            move = np.array([px-pos[0][0], py-pos[0][1]])
+            move =  move/norm(move)*0.5
+            px = px - move[0]
+            py = py - move[1]
+
+        if tag != rid:
+            px = px+(random()-0.5)
+            py = py+(random()-0.5)
+
+        course = course+(random()-0.5)/5.0
+        client.setPosition(px, py, course)

robot.py

@@ -0,0 +1,61 @@
+import numpy as np
+
+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 self.ccw(A,C,D) != self.ccw(B,C,D) and self.ccw(A,B,C) != self.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 other:
+                if self.line_intersect(i[0], i[1], j[0], j[1]):
+                    return True
+        return False

server.py

@@ -8,7 +8,6 @@ import cv2
 import time
 import numpy as np
 import os
-from numpy.linalg import norm
 from cmarkers import getCMarkers, get_transform_matrix_points
  
 ##
@@ -154,7 +153,7 @@ def get_green_dots(img_orig):
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
     cam_id = 0
-    dev = open_camera(cam_id)
+    dev = None
     
     parser.add_argument('-a',
                         '--automatic',
@@ -196,11 +195,11 @@ if __name__ == "__main__":
     server = tag.TagServer(10318, board_size)
 
     while True:
-        img_orig = np.zeros(transform_size)
+        img_orig = np.zeros([transform_size[0], transform_size[1], 3])
         if camera:
             img_orig = get_frame(dev)
         else:
-            time.sleep(0.01)
+            time.sleep(0.1)
 
         if img_orig is not None: # if we did get an image
             img = img_orig
@@ -224,14 +223,25 @@ if __name__ == "__main__":
                 for line in robot.lines():
                     p1 = (line[0]*dpi).astype(int)
                     p2 = (line[1]*dpi).astype(int)
-                    cv2.line(img, (p1[0], p1[1]), (p2[0], p2[1]), (255, 255, 0))
+                    color = (255, 255, 0)
+                    if robot == server.tag:
+                        color = (0, 0, 255)
+                    cv2.line(img, (p1[0], p1[1]), (p2[0], p2[1]), color)
+            if server.paused:
+                cv2.putText(img, "PAUSE", (0,30), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255))
             cv2.imshow("warped", img)
  
         else: # if we failed to capture (camera disconnected?), then quit
             break
  
-        if (cv2.waitKey(2) >= 0):
+        k = cv2.waitKey(33)
+        if k == 115: # s
+            server.startGame()
+        elif k == 112: # p
+            server.stopGame()
+        elif k == 113: # q
             server.stop()
             break
- 
-    cleanup(cam_id)
+
+    if camera:
+        cleanup(cam_id)

tag.py

@@ -4,6 +4,9 @@ import threading
 import numpy as np
 from socket import error as SocketError
 from robot import Robot
+import time
+
+run_time = 5
 
 class TagServer(threading.Thread):
     def __init__(self, port, board_size):
@@ -22,11 +25,19 @@ class TagServer(threading.Thread):
         self.tag = None
         self.board_size = board_size
 
+        self.paused = True
         self.runV = True
+
+        self.last = 0
+        self.mutex = threading.Lock()
+        self.mutex2 = threading.Lock()
         self.start()
 
     def updatePosition (self, idd, posx, posy, course):
-        for thread in self.clients_threads:
+        self.mutex.acquire()
+        clients_threads = self.clients_threads
+        self.mutex.release()
+        for thread in clients_threads:
             if thread.robot is not None and thread.robot.idd == idd:
                 msg = "P "
                 msg = msg + str(posx)+","
@@ -43,6 +54,7 @@ class TagServer(threading.Thread):
                 self.brodcast(msg)
 
         # check for collision
+        self.mutex2.acquire()
         for i in self.robots():
             if i != self.tag:
                 continue
@@ -50,24 +62,32 @@ class TagServer(threading.Thread):
                 if i == j:
                     continue
                 if i.intersect(j):
-                    self.tag = j
-                    self.brodcast("RUN "+str(self.tag.id)+" 10")
-
-    def startGame (self):
+                    if self.last + run_time <= time.time():
+                        print(i.idd, " pegou ", j.idd)
+                        self.tag = j
+                        self.brodcast("RUN "+str(self.tag.idd)+","+str(run_time))
+                        self.last = time.time()
+                        break
+        self.mutex2.release()
+
+    def stopGame (self):
+        self.paused = True
         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
+        r = self.robots()
+        if len(r) >= 1:
+            tag = np.random.choice(r, 1)
+            self.tag = tag[0]
+            self.paused = False
+        self.brodcast("RUN "+str(self.tag.idd)+","+str(run_time))
 
     def brodcast(self, msg):
-        for thread in self.clients_threads:
+        self.mutex.acquire()
+        clients_threads = self.clients_threads
+        self.mutex.release()
+        for thread in clients_threads:
             if thread.robot is not None:
                 thread.send(msg+"\n")
 
@@ -76,20 +96,29 @@ class TagServer(threading.Thread):
             try:
                 (clientsock, (ip, port)) = self.sock.accept()
                 newthread = TagThread(ip, port, clientsock, self)
+                self.mutex.acquire()
                 self.clients_threads.append(newthread)
+                self.mutex.release()
+                self.stopGame()
             except socket.timeout as err:
                 pass
         self.sock.close()
 
     def robots(self):
         r = []
-        for thread in self.clients_threads:
+        self.mutex.acquire()
+        clients_threads = self.clients_threads
+        self.mutex.release()
+        for thread in clients_threads:
             if thread.robot is not None:
                 r.append(thread.robot)
         return r
 
     def stop(self):
-        for thread in self.clients_threads:
+        self.mutex.acquire()
+        clients_threads = self.clients_threads
+        self.mutex.release()
+        for thread in clients_threads:
             thread.stop()
         self.runV = False
 
@@ -110,13 +139,14 @@ class TagThread(threading.Thread):
 
         socket.settimeout(1)
         self.runV = True
+        self.mutex = threading.Lock()
         self.start()
 
 
     def run(self):
         while self.runV:
             try:
-                data = self.sock.recv(64).decode('ascii')
+                data = self.sock.recv(128).decode('ascii')
             except socket.timeout as err:
                 continue
             except SocketError as e:
@@ -124,6 +154,7 @@ class TagThread(threading.Thread):
                 break
             data = data.split(" ")
             try:
+                # print (data)
                 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))
@@ -133,62 +164,123 @@ class TagThread(threading.Thread):
                     posx, posy, curse = float(posx), float(posy), float(curse)
                     self.server.updatePosition(self.robot.idd, posx, posy, curse)
             except Exception as e:
-                print(e)
+                raise
                 self.send("ERR\n")
         self.sock.close()
 
     def send(self, msg):
+        self.mutex.acquire()
         self.sock.sendall(msg.encode('ascii'))
+        self.mutex.release()
 
     def stop(self):
         self.runV = False
 
 class TagClient(threading.Thread):
-    def __init__(sself, ip, port):
+    def __init__(self, ip, port):
+        threading.Thread.__init__(self)
         sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
         server_address = (ip, port)
         sock.connect(server_address)
+        sock.settimeout(1)
         self.sock = sock
-        self.stop = None
-        self.start = None
-        self.tagger = None
+        self.stopf = None
+        self.startf = None
+        self.runf = None
         self.position = None
 
+        # info
+        self.runing = False
+        self.tag = None
+        self.positions = []
+        self.board = None
+
+        self.mutex = threading.Lock()
+
     def info (self, idd, xsize, ysize, xtag, ytag):
+        idd, xsize, ysize, xtag, ytag = str(idd), str(xsize), str(ysize), str(xtag), str(ytag)
         message = "SUB "+idd+","+xsize+","+ysize+","+xtag+","+ytag+"\n"
         self.sock.sendall(message.encode('ascii'))
+        self.runV = True
         self.start()
 
     def run(self):
-        while True:
+        data = ""
+        while self.runV:
             try:
-                data = self.sock.recv(64).decode('ascii')
+                data += self.sock.recv(256).decode('ascii')
+            except socket.timeout as err:
+                continue
             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
+            if "\n" not in data:
+                continue
 
-    def startCallback (self, function):
-        self.start = function
+            tmp = data.split("\n")
+            if tmp[-1] != '':
+                data = tmp[-1]
+                tmp = tmp[0:-1]
+            else:
+                data = ""
+
+            for d in tmp:
+                d = d.split(" ")
+                if d[0] == "OK":
+                    x, y = d[1].split(",")
+                    x, y = float(x), float(y)
+                    self.board = (x, y)
+
+                elif d[0] == "P":
+                    posx, posy, course, idd, xsize, ysize, xtag, ytag = d[1].split(",")
+                    posx = float(posx)
+                    posy = float(posy)
+                    course = float(course)
+                    idd = int(idd)
+                    xsize = float(xsize)
+                    ysize = float(ysize)
+                    xtag = float(xtag)
+                    ytag = float(ytag)
+
+                    self.mutex.acquire()
+                    self.positions.append((posx, posy, course, idd, xsize, ysize, xtag, ytag))
+                    self.mutex.release()
+
+                elif d[0] == "START":
+                    print(d)
+                    self.runing = True
+
+                elif d[0] == "STOP":
+                    print(d)
+                    self.runing = False
+
+                elif d[0] == "RUN":
+                    print(d)
+                    idd, interval = d[1].split(",")
+                    self.tag = (int(idd), float(interval)+time.time())
+        self.sock.close()
 
-    def taggerCallback (self, function):
-        self.tagger = function
+    def getStatus(self):
+        return self.runing
 
-    def positionCallback (self, function):
-        self.position = function
+    def getBoard(self):
+        return self.board
 
-    def boardinfoCallback (self, function):
-        self.boardinfo = function
+    def getPosition(self):
+        i = self.positions
+        self.mutex.acquire()
+        self.positions = []
+        self.mutex.release()
+        return i
+
+    def getTagInfo(self):
+        return self.tag
 
     # Somente na simulacao
-    def setPosition(px, py):
-        message = "P "+px+","+py+"\n"
+    def setPosition(self, px, py, course):
+        message = "P "+str(px)+","+str(py)+","+str(course)+"\n"
         self.sock.sendall(message.encode('ascii'))
+
+    def stop(self):
+        self.runV = False

test.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+#Lists various information from all bricks it can connect to.
+
+import sys, traceback
+import nxt.locator
+import nxt.brick
+
+
+b = None
+try:
+    print('Find brick...')
+    b = nxt.locator.find_one_brick()
+    name, host, signal_strength, user_flash = b.get_device_info()
+    print('NXT brick name: %s' % name)
+    print('Host address: %s' % host)
+    print('Bluetooth signal strength: %s' % signal_strength)
+    print('Free user flash: %s' % user_flash)
+    prot_version, fw_version = b.get_firmware_version()
+    print('Protocol version %s.%s' % prot_version)
+    print('Firmware version %s.%s' % fw_version)
+    millivolts = b.get_battery_level()
+    print('Battery level %s mV' % millivolts)
+    b.play_tone_and_wait(300, 50)
+    b.play_tone_and_wait(400, 50)
+    b.play_tone_and_wait(500, 50)
+    b.play_tone_and_wait(600, 50)
+    print('done')
+    b.sock.close()
+except:
+    print("Error while running test:")
+    traceback.print_tb(sys.exc_info()[2])
+    print(str(sys.exc_info()[1]))
+    if b in locals():
+        b.sock.close