5DDGu41y

1import numpy as np
2import cv2
3import os
4import copy
5import math
6import itertools
7from Field import *
8from TSP import *
9from FieldPoint import *
10import socket
11from Drive import *
12
13# open the camera (You can uncomment this line when switching to camera input)
14cap = cv2.VideoCapture(0)
15
16client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # Open a client socket
17
18dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_5X5_250)
19#markerImage = cv2.aruco.drawMarker(dictionary, 26, 200)
20#cv2.imwrite("marker4.jpg",markerImage) 
21#cv2.imshow('marker', markerImage)
22def getCorners(detectedMarkers, markerID): #Markerid = detected[1]/
23    indexOfCoordinates = detectedMarkers[1].tolist().index([markerID])
24    #print(detectedMarkers[0][indexOfCoordinates][0])
25    return detectedMarkers[0][indexOfCoordinates][0]
26
27def getCenter(detectedMarkers, markerID):
28    listOfFourCoordinates = getCorners(detectedMarkers, markerID)
29    x = (listOfFourCoordinates[0][0] + listOfFourCoordinates[2][0])/2
30    y = (listOfFourCoordinates[0][1] + listOfFourCoordinates[2][1])/2
31    return (int(x), int(y))
32    
33fill = 0
34field = [[copy.deepcopy(fill) for x in range(64)] for y in range(48)]    
35#f = Field(60*fieldX, 60*fieldY, fieldY, fieldX) # initializing field object. See file Field.py
36f = Field(64*10, 48*10, 48, 64)
37f.createWindow()
38f.update(field)       
39
40#read the image from the camera (You can uncomment this line when switching to camera input)
41ret, frame = cap.read() 
42#read the image from file (You should comment out this line when switching to camera input)
43#frame = cv2.imread('example.jpg', 0)
44cv2.imshow('original', frame)
45
46#This function detects all the markers from the current frame
47# The returned structure is a list, where:
48# First element (index 0) is a tuple of ([list of marker corner coordinates (as lists first element)], [type of varaible the coordinates are represented in])
49# Second element is list of marker id numbers, each as a list.
50# the lists are in same order (when using coordinate list like detected[0][0][0][0][0], then this is the first coordinate of marker with id detected[1][0][0])
51detected = cv2.aruco.detectMarkers(frame, dictionary)
52##print(detected[0]) #print the list of detected id numbers
53
54#This function prints the list of markers onto the frame (their main corner, contour and id)
55cv2.aruco.drawDetectedMarkers(frame, detected[0], detected[1])
56#PRINT THE CENTER POINT OF THE MARKER THAT IS ON THE ROBOT TO THE TERMINAL.
57###print(getCenter(detected, 20))
58
59t = TSP()               ############Create a new TSP algorithm solver##############
60markersIDs = []
61######FILTER OUT OBSTACLES (marker id greater than 65)#######
62for el in detected[1]:
63    if el[0] > 65:
64        fourCoordinates = getCorners(detected, el[0])
65        minX = math.floor(min(fourCoordinates[0][0], fourCoordinates[1][0], fourCoordinates[2][0], fourCoordinates[3][0])/10)
66        maxX = math.ceil(max(fourCoordinates[0][0], fourCoordinates[1][0], fourCoordinates[2][0], fourCoordinates[3][0])/10)
67        minY = math.floor(min(fourCoordinates[0][1], fourCoordinates[1][1], fourCoordinates[2][1], fourCoordinates[3][1])/10)
68        maxY = math.ceil(max(fourCoordinates[0][1], fourCoordinates[1][1], fourCoordinates[2][1], fourCoordinates[3][1])/10)
69        
70        for el in range(int(minY)-2,int(maxY)+2):
71            for el2 in range(int(minX)-2,int(maxX)+2):
72                field[el][el2] = 1
73    elif el[0] != 4:
74        markersIDs.append(el[0])
75        t.add_point(el[0])
76        
77def aStarAlgorithm(field):
78    fieldX = len(field[0])
79    fieldY = len(field)
80    
81    nodes = np.empty((fieldX, fieldY), dtype=object) # Empty numpy array of nodes. Numpy array allows array of objects
82    open = [] # Empty array for opened nodes
83    closed = [] # Empty array for closed nodes
84    start = None # Empty pointer for start node
85    finish = None # Empty pointer for finish node
86    
87    for i, row in enumerate(field):
88        for j, item in enumerate(row):
89            if item == 1:
90                nodes[j, i] = FieldPoint(j, i, Type=item, Passable=False) # Walls are not passable
91            else:
92                nodes[j, i] = FieldPoint(j, i, Type=item)
93                if item == 2:
94                    start = FieldPoint(j, i)
95                if item == 3:
96                    finish = FieldPoint(j, i)
97
98    # Set up starting node
99    start.gScore = 0
100    open.append(start)
101
102    # Find path using A* algorithm
103    while len(open) > 0 and finish not in closed:
104        # Write you code here (before the following line):
105        currentNode = open.pop(0)
106        closed.append(currentNode)
107        
108        for posX in [currentNode.x -1, currentNode.x, currentNode.x +1]:
109            if posX < 0 or posX > fieldX - 1:
110                continue
111            for posY in [currentNode.y -1, currentNode.y, currentNode.y +1]:
112                if posY < 0 or posY > fieldY - 1:
113                    continue
114                if currentNode == nodes[posX][posY]:
115                    continue
116                if nodes[posX][posY].passable and nodes[posX][posY] not in closed:
117                    if nodes[posX][posY] in open:
118                        if currentNode.gScore + currentNode.dist(nodes[posX][posY]) < nodes[posX][posY].gScore:
119                            nodes[posX][posY].setParent(currentNode)
120                            nodes[posX][posY].gScore = currentNode.gScore + currentNode.dist(nodes[posX][posY])
121                            nodes[posX][posY].fScore = nodes[posX][posY].gScore + nodes[posX][posY].dist(finish)
122                    else:
123                        nodes[posX][posY].setParent(currentNode)
124                        nodes[posX][posY].gScore = currentNode.gScore + currentNode.dist(nodes[posX][posY])
125                        nodes[posX][posY].fScore = nodes[posX][posY].gScore + nodes[posX][posY].dist(finish)
126                        open.append(nodes[posX][posY])    
127    # Draw the path (go from finish to start using parents)
128    currentPath = finish # Start of the reverse path finding
129    pathForTSP = []
130    while not currentPath.equals(start): # If start is reached the path is found
131        index = closed.index(currentPath)
132        pathForTSP.append((currentPath.x, currentPath.y)) #
133        if closed[index].type != 3: # Make all steps except the finish to a path
134            closed[index].type = 5
135        currentPath = closed[index].parent # Take the next point
136    
137    return pathForTSP
138# Display the resulting image with markers
139cv2.imshow('markers', frame)
140f.update(field)
141
142t.add_start_point(4) #EXAMPLE FROM TSP_DEMO.PY / alguse marker id = 20 /// ROBOTI MARKER 4!
143markersIDs.insert(0,4)
144
145for pair in list(itertools.combinations(markersIDs, 2)):
146    field2 = copy.deepcopy(field)
147    #print("foo1")
148    start = (getCenter(detected,pair[0])[0]//10, getCenter(detected,pair[0])[1]//10) #X,Y CENTER COORDINATES
149    destination = (getCenter(detected,pair[1])[0]//10, getCenter(detected,pair[1])[1]//10) #X,Y CENTER COORDINATES
150    print(start)
151    #print(destination)
152    field2[start[1]][start[0]] = 2 #Y FIRST, THEN X (field.py)
153    field2[destination[1]][destination[0]] = 3 #Y FIRST, THEN X (field.py)
154    #print(field2)
155    t.add_weight(pair[0], pair[1], aStarAlgorithm(field2))
156    
157    
158(shortest_dist, shortest_order, shortest_path) = t.shortest_path() #function to find the shortest distance to pass the points
159
160print(shortest_path)
161#print("foo2")
162
163for el in shortest_path:
164    #print("foo3")
165    field[el[1]][el[0]] = 5
166      
167f.update(field)
168
169drive = Drive(shortest_path) 
170try:
171    while True:
172        ret, frame = cap.read()
173        cv2.imshow('original', frame)
174        detected = cv2.aruco.detectMarkers(frame, dictionary)
175
176        abc = getCorners(detected, 4)  
177        order = drive.get_action(abc[0], abc[2])
178
179        data = order
180        
181        # IP_ADRESS = Your raspberry IP address (You can check it by using 'ifconfig' command on raspberry)
182        # 66xx = Port number. Change it to 66xx where xx is the number of your SD card following the 'R' (use 0x for 1-digit numbers (use 04 for R4))
183        IP_ADDRESS = "192.168.1.122" # for example "192.168.15.23"
184        PORT_NR = 6614
185        
186        if IP_ADDRESS == "192.168.15.23":
187            raise ValueError("You did not change the IP address to your raspberry pi IP")
188        if PORT_NR == 6666:
189            raise ValueError("You did not change the port number to 66xx, where xx is the number part of your SD card number")
190
191        client_socket.sendto(data.encode('utf-8'), (IP_ADDRESS,PORT_NR)) # Send data out
192        print ("Sending request")
193        
194except Exception as e:
195    # Close the socket if any error occurs, also print the error message to know what happened
196    print(e)
197    client_socket.close()
198
199except KeyboardInterrupt:
200    # Also close the socket if the program is closed
201    print("exiting")
202    client_socket.close()
203print('done')
204# Quit the program when Q is pressed
205while True:
206    if cv2.waitKey(1) & 0xFF == ord('q'):
207            break
208                
209
210# When everything done, release the capture
211
212print('closing program')
213try:
214        cap.release()
215except:
216        pass
217cv2.destroyAllWindows()
218#writetoFile("threshold.txt")