├── README.md ├── Report └── suru_iha_teknofest_2021.pdf ├── media ├── square-formation.gif ├── swarm.JPG └── take-off-square.gif └── src ├── formations.py ├── menu.py ├── sub_functions.py └── swarm_class.py /README.md: -------------------------------------------------------------------------------- 1 | # Swarm-UAV 2 | 3 |
4 |

5 | 6 |

7 |
8 | 9 | ## Contents 10 | - [Project Purpose](#PP) 11 | - [Installation](#Installation) 12 | - [Features](#Features) 13 | - [Simulation](#Simulation) 14 | - [Menu](#Menu) 15 | - [Algorithms](#Algorithms) 16 | 17 | ## Project Purpose 18 | The need for Swarm Unmanned Aerial Vehicles (Swarm UAV), one of the most important technologies of the near future, is increasing day by day. Swarm UAVs show their importance with both civil and military applications. For example, in a civilian scenario, it will be possible to intervene more effectively in forest fires with Swarm UAVs, and in a military scenario, it will be possible to gain superiority by developing alternative strategies and tactics against existing manned air, land and sea platforms. The main purpose of the Swarm UAV Competition is to compete.Developing software algorithms for the creation of UAVs that can perform the tasks defined in the specification in swarms, demonstrating the performances of these algorithms with Swarm UAVs in the physical environment, as well as guiding young people on the subject and increasing their experience and knowledge. 19 | 20 | 21 | ## Installation 22 | - This project is developed in crazyswarm (see: https://crazyswarm.readthedocs.io/en/latest/index.html) 23 | - Firstly you should install Ros (see: https://www.ros.org/) 24 | - Then you should install crazyswarm. We will use just python api. 25 | 26 | ### How to install Crazyswarm for just Python API 27 | (https://crazyswarm.readthedocs.io/en/latest/installation.html) 28 | 29 | 1- __Set enviroment veriable__ 30 | ``` 31 | $ export CSW_PYTHON=[python2 or python3] 32 | ``` 33 | 2- __Install depencies__ 34 | ``` 35 | $ sudo apt install git make gcc swig lib${CSW_PYTHON}-dev ${CSW_PYTHON}-numpy ${CSW_PYTHON}-yaml ${CSW_PYTHON}-matplotlib 36 | ``` 37 | 3- __Clone Repo__ 38 | ``` 39 | $ git clone https://github.com/USC-ACTLab/crazyswarm.git 40 | ``` 41 | 4- __Run build script__ 42 | ``` 43 | $ cd crazyswarm 44 | $ ./buildSimOnly.sh 45 | ``` 46 | 47 | ## Features 48 | - UAV Individual take off, formation in the air 49 | - Take off with Formation 50 | - Adding a new individual to the Swarm 51 | - Removal of individual from the Swarm 52 | - Changing Swarm direction with formation protection 53 | - Formation change 54 | - Swarm separation 55 | - Swarm merge 56 | - Swarm navigation 57 | - Ascent and descent with swarm 58 | - Simultaneous / sequential auto-landing with Swarm 59 | 60 | ## Simulation 61 |

62 | 63 |
64 | 65 |

66 | 67 | 68 | 69 | 70 | ## Menu 71 | - It is a control menu created to simulate the developed algorithms. 72 | 73 | #### ![ss](https://i.hizliresim.com/j5cm3q1.png) 74 | - First of all, choose drone number. 75 | - Second, choose whether the drones will take off simultaneously or individually. 76 | - Choose how many meters they will rise. 77 | - Choose the distance between drones. 78 | #### ![ss1](https://i.hizliresim.com/eiz26zi.png) 79 | - The formation that can be entered is selected according to the number of drones. 80 | #### ![ss2](https://i.hizliresim.com/rdgj1p0.png) 81 | - Different tasks can be performed with the specified numbers. 82 | 83 | 84 | ## Algorithms 85 | #### -set_triang_formation 86 | 87 | 88 | 89 | The purpose of this function is to create a triangle formation with the UAVs in the system
90 |       --Parameters the Function Takes Are
91 |              1- distance = is the distance between the two vertices of the triangle you want to form
92 |              2- drone_number = number of drones in the system
93 |              3- init_point = center at the time the swarm members call a function
94 |              4- altitude = height of swarm members
95 |              5- rotation = angles of swarm members with respect to the main coordinate system
96 | 97 |       --Description Of the Algorithm
98 |              Step 1 = 3x3 unit matrix named desired_matrix is created for the three corners of the triangle to be created. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.

99 |               Step 2 = 3 line arrays are created for the side lines of the triangle, but they are empty. Will be resized.

100 |               Step 3 = On the relative coordinate system on which the swarm system is located, the corner points of the triangle are calculated using geometric relations and assigned to the desired_matrix.

101 |               Step4 = If there are more than three UAVs in the swarm, they are placed on the border lines by editing the line variable with the re_construct_line function(The re_construct_line function will be explained later).

102 |               Step5 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.

103 |               Step 6 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.

104 | 105 | 106 | #### -set_square_formation: 107 | 108 | The purpose of this function is to create a square formation with the UAVs in the system.
109 |        --Parameters the Function Takes Are:
110 |              1- distance = is the distance between the two vertices of the square you want to form.
111 |              2- drone_number = number of drones in the system
112 |              3- init_point = center at the time the swarm members call a function
113 |              4- altitude = height of swarm members
114 |              5- rotation = angles of swarm members with respect to the main coordinate system
115 | 116 |       --Description Of the Algorithm:
117 |              Step 1 = 4x3 unit matrix named desired_matrix is created for the four corners of the square to be created. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
118 |              Step 2 = 4 line arrays are created for the side lines of the square, but they are empty. Will be resized.
119 |              Step 3 = On the relative coordinate system on which the swarm system is located, the corner points of the square are calculated using geometric relations and assigned to the desired_matrix.
120 |              Step4 = If there are more than four UAVs in the swarm, they are placed on the border lines by editing the line variable with the re_construct_line function(The re_construct_line function will be explained later).
121 |              Step5 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
122 |              Step 6 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
123 | 124 | 125 | 126 | #### -set_v_formation 127 | 128 | The purpose of this function is to create a ‘v’ formation with the UAVs in the system.
129 |        --Parameters the Function Takes Are:
130 |              1- distance = is the distance between the two vertices of the ‘v’ you want to form.
131 |              2- yaw_angle = is the angle between two lines.
132 |              3- drone_number = number of drones in the system
133 |              4- init_point = center at the time the swarm members call a function
134 |              5- altitude = height of swarm members
135 |              6- rotation = angles of swarm members with respect to the main coordinate system
136 | 137 |       --Description Of the Algorithm:
138 |              Step 1 = (drone_number)x3 unit matrix named desired_matrix is created for each element of the formation to be created. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
139 |              Step 2 = On the relative coordinate system on which the swarm system is located, the lines of the v formation are calculated using geometric relations and assigned to the desired_matrix.
140 |              Step 3 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
141 |              Step 4 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
142 | 143 | 144 | 145 | #### -set_pentagon_formation: 146 | 147 | The purpose of this function is to create a pentagon formation with the UAVs in the system.
148 |        --Parameters the Function Takes Are:
149 |              1- distance = is the distance between the two vertices of the pentagon you want to form.
150 |              2- drone_number = number of drones in the system
151 |              3- init_point = center at the time the swarm members call a function
152 |              4- altitude = height of swarm members
153 |              5- rotation = angles of swarm members with respect to the main coordinate system
154 | 155 |       --Description Of the Algorithm:
156 |              Step 1 = 5x3 unit matrix named desired_matrix is created for the five corners of the pentagon to be created. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
157 |              Step 2 = 5 line arrays are created for the side lines of the pentagon, but they are empty. Will be resized.
158 |              Step 3 = On the relative coordinate system on which the swarm system is located, the corner points of the pentagon are calculated using geometric relations and assigned to the desired_matrix.
159 |              Step4 = If there are more than five UAVs in the swarm, they are placed on the border lines by editing the line variable with the re_construct_line function(The re_construct_line function will be explained later).
160 |              Step5 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
161 |              Step 6 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
162 | 163 | #### -set_star_formation: 164 | 165 | The purpose of this function is to create a star formation with the UAVs in the system.
166 |        --Parameters the Function Takes Are:
167 |              1- distance = is the distance between the two vertices of the star you want to form.
168 |              2- drone_number = number of drones in the system
169 |              3- init_point = center at the time the swarm members call a function
170 |              4- altitude = height of swarm members
171 |              5- rotation = angles of swarm members with respect to the main coordinate system
172 | 173 |       --Description Of the Algorithm:
174 |              Step 1 = 10x3 unit matrix named desired_matrix is created for the ten corners of the star to be created. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
175 |              Step 2 = 10 line arrays are created for the side lines of the star, but they are empty. Will be resized.
176 |              Step 3 = On the relative coordinate system on which the swarm system is located, the corner points of the star are calculated using geometric relations and assigned to the desired_matrix.
177 |              Step4 = If there are more than ten UAVs in the swarm, they are placed on the border lines by editing the line variable with the re_construct_line function(The re_construct_line function will be explained later).
178 |              Step5 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
179 |              Step 6 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
180 | 181 | #### -set_crescent_formation: 182 | 183 | The purpose of this function is to create a crescent formation with the UAVs in the system.
184 |        --Parameters the Function Takes Are:
185 |              1-radius = determines the radius of the crescent that will be created
186 |              2- gap_angle = represents the angle value of the part of the crescent that will be empty
187 |              3- drone_number = number of drones in the system
188 |              4- init_point = center at the time the swarm members call a function
189 |              5- altitude = height of swarm members
190 |              6- rotation = angles of swarm members with respect to the main coordinate system
191 | 192 |       --Description Of the Algorithm:
193 |              Step1 = 2 endpoints of the crescent are determined
194 |              Step 2 = (drone_number)x3 unit matrix named desired_matrix is created to generate the crescent line. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
195 |              Step 3 =In the while loop, the coordinates of each UAV are calculated according to the angle values and transferred to the desired_matrix.
196 |              Step4 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
197 |              Step5 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
198 | 199 | 200 | #### -set_circle_formation: 201 | The purpose of this function is to create a cicle formation with the UAVs in the system.
202 |        --Parameters the Function Takes Are:
203 |              1-radius = determines the radius of the crescent that will be created
204 |              2- drone_number = number of drones in the system
205 |              3- init_point = center at the time the swarm members call a function
206 |              4- altitude = height of swarm members
207 |              5- rotation = angles of swarm members with respect to the main coordinate system
208 | 209 |       --Description Of the Algorithm:
210 |              Step1 = The first point is passed to the variable point 1
211 |              Step 2 = (drone_number)x3 unit matrix named desired_matrix is created to generate the circle line. Where the rows represent UAVs. The 1st column represents the x-axis, the 2nd column represents the y-axis, and the 3rd column represents the z-axis.
212 |              Step 3 = According to the radius value received and the number of UAVs, the positions to be reached in the while loop are calculated and transferred to the desired_matrix variable.
213 |              Step4 = All of the specified coordinates are assigned to the desired_matrix variable and the transformed_matrix is created.
214 |              Step5 = All coordinates are arranged according to the absolute frame with the transformation_2d function and transferred to the transformed_matrix.
215 | 216 | -------------------------------------------------------------------------------- /Report/suru_iha_teknofest_2021.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/okantorun/Swarm-UAV/ac2278a2c9a5852f0366d18aee960b76fb8c21ce/Report/suru_iha_teknofest_2021.pdf -------------------------------------------------------------------------------- /media/square-formation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/okantorun/Swarm-UAV/ac2278a2c9a5852f0366d18aee960b76fb8c21ce/media/square-formation.gif -------------------------------------------------------------------------------- /media/swarm.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/okantorun/Swarm-UAV/ac2278a2c9a5852f0366d18aee960b76fb8c21ce/media/swarm.JPG -------------------------------------------------------------------------------- /media/take-off-square.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/okantorun/Swarm-UAV/ac2278a2c9a5852f0366d18aee960b76fb8c21ce/media/take-off-square.gif -------------------------------------------------------------------------------- /src/formations.py: -------------------------------------------------------------------------------- 1 | from sub_functions import * 2 | 3 | def set_triang_formation(distance,drone_number,init_point,altitude,rotation): 4 | desired_matrix = np.zeros((3,3)) 5 | line1 = np.zeros((0,3)) 6 | line2 = np.zeros((0,3)) 7 | line3 = np.zeros((0,3)) 8 | d = distance 9 | point_a = [0,d/m.sqrt(3),altitude] 10 | point_b = [-d*m.sin(m.pi/6),-d*m.cos(m.pi/6)+d/m.sqrt(3),altitude] 11 | point_c = [d*m.sin(m.pi/6),-d*m.cos(m.pi/6)+d/m.sqrt(3),altitude] 12 | desired_matrix[0] = np.array(point_a) 13 | desired_matrix[1] = np.array(point_b) 14 | desired_matrix[2] = np.array(point_c) 15 | 16 | i=3 17 | while i 3 17 | Square --> 4 18 | V --> 5 19 | Pentagon --> 5 20 | Crescent --> 6 21 | Star --> 10 22 | 23 | """) 24 | 25 | drone_number = int(input("Please enter how many drones you want to start with : ")) 26 | print("------------------------------------------------") 27 | print("------------------------------------------------") 28 | take_off_type = int(input("""If you want simultaneous take off of drones --> 1 29 | Teker teker kalkmalarını istiyorsanız --> 2 : 30 | """)) 31 | hight = float(input("how many meters high?: ")) 32 | distance = float(input("enter the distance between elements : ")) 33 | 34 | swarm = Crazyswarm() 35 | timeHelper = swarm.timeHelper 36 | allcfs = swarm.allcfs 37 | swarm.allcfs.takeoff(0,0) 38 | crazyflies = allcfs.crazyflies[0:drone_number] 39 | swarm2 = Swarm(crazyflies,timeHelper,allcfs) 40 | 41 | if take_off_type == 1 : 42 | swarm2.takeoff_ascyn(hight, 5) 43 | elif take_off_type == 2: 44 | swarm2.takeoff_scyn(hight, 5) 45 | else : 46 | print("you entered wrong") 47 | 48 | 49 | 50 | if drone_number == 3: 51 | print("formations to choose from : ") 52 | print("1 --> Triangle ") 53 | elif drone_number ==4 : 54 | print("formations to choose from : ") 55 | print("1 --> Triangle ") 56 | print("2 --> Square ") 57 | elif drone_number ==5 : 58 | print("formations to choose from : ") 59 | print("1 --> Triangle ") 60 | print("2 --> Square ") 61 | print("3 --> V formation ") 62 | print("4 --> Pentagon formation ") 63 | elif drone_number == 6 : 64 | print("formations to choose from : ") 65 | print("1 --> Triangle ") 66 | print("2 --> Square ") 67 | print("3 --> V formation ") 68 | print("4 --> Pentagon formation ") 69 | print("5 --> Crescent formation ") 70 | 71 | elif drone_number > 6 : 72 | print("formations to choose from : ") 73 | print("1 --> Triangle ") 74 | print("2 --> Square ") 75 | print("3 --> V formation ") 76 | print("4 --> Pentagon formation ") 77 | print("5 --> Crescent formation ") 78 | print("6 --> Star formation") 79 | 80 | 81 | formation_id = int(input("enter formation number : ")) 82 | 83 | 84 | if formation_id == 1: 85 | swarm2.take_formation(distance,formation_id,hight,0,0) 86 | elif formation_id == 2: 87 | swarm2.take_formation(distance,formation_id,hight,0,0) 88 | elif formation_id == 3: 89 | swarm2.take_formation(distance,formation_id,hight,0,0) 90 | elif formation_id == 4: 91 | swarm2.take_formation(distance,formation_id,hight,0,0) 92 | elif formation_id == 5: 93 | swarm2.take_formation(distance,formation_id,hight,0,0) 94 | elif formation_id == 6: 95 | swarm2.take_formation(distance,formation_id,hight,0,0) 96 | elif formation_id == 7: 97 | swarm2.take_formation(distance,formation_id,hight,0,0) 98 | 99 | cur_x = 0 100 | cur_y = 0 101 | 102 | while True : 103 | print("""To update the formation --> 1 104 | For Ascension or Descent --> 2 105 | For rotation --> 3 106 | For navigation --> 4 107 | For swarm separation --> 5 108 | For Turning Navigation --> 6 109 | For sequential landing --> 7 110 | """) 111 | selection = int(input("Selection : ")) 112 | if selection == 1 : 113 | print(""" to add an element --> 1 114 | to remove element --> 2 115 | to update the distance between elements --> 3 116 | to change formation --> 4 117 | for adding elements while in square formation --> 5 118 | """) 119 | selection2 = int(input("Selection : ")) 120 | 121 | if selection2 == 1 : 122 | swarm2.add_crazyflie(allcfs.crazyflies[drone_number]) 123 | timeHelper.sleep(5) 124 | drone_number +=1 125 | swarm2.take_formation(distance,formation_id,hight,cur_x,cur_y) 126 | elif selection2 == 2 : 127 | rastgele = random.randint(0,drone_number-1) 128 | print("randomly removing element ...") 129 | swarm2.remove_crazyflie(swarm2.crazyflies[rastgele]) 130 | drone_number = drone_number -1 131 | swarm2.take_formation(distance,formation_id,hight,cur_x,cur_y) 132 | elif selection2 == 3 : 133 | aralık = int(input("Enter the distance you want between the elements : ")) 134 | distance = aralık 135 | swarm2.take_formation(aralık,formation_id,hight,cur_x,cur_y) 136 | elif selection2 == 4 : 137 | print("formations to choose from : ") 138 | print("1 --> Triangle ") 139 | print("2 --> Square ") 140 | print("3 --> V formation ") 141 | print("4 --> Pentagon formation ") 142 | print("5 --> Crescent formation ") 143 | print("6 --> Star formation") 144 | print("7 --> Circle formation") 145 | 146 | selection3 = int(input("Selection : ")) 147 | formation_id = selection3 148 | swarm2.change_formation(selection3,distance,cur_x,cur_y) 149 | 150 | elif selection == 2 : 151 | vertical_move = float(input("To what height do you want to move the Swarm : ")) 152 | swarm2.takeoff_ascyn(vertical_move, 5) 153 | hight = vertical_move 154 | 155 | elif selection == 3 : 156 | rot = int(input("How many degrees do you want to rotate? : ")) 157 | print(cur_x) 158 | print(cur_y) 159 | swarm2.rotasyon_3(rot,cur_x, cur_y, hight,distance) 160 | 161 | 162 | elif selection == 4 : 163 | x = float(input("Enter the x coordinate of the point you want to navigate : " )) 164 | y = float(input("Enter the y-coordinate of the point you want to navigate : " )) 165 | swarm2.navigation_2([x,y,hight],10) 166 | cur_x = x 167 | cur_y = y 168 | 169 | elif selection == 5 : 170 | print("There must be a minimum of 6 UAVs to make a swarm separation ") 171 | if drone_number > 5 : 172 | print("A triangle for testing and another pattern we want : ") 173 | sep_swarm = int(input("Please enter the other formation you want ")) 174 | crazyflies_1 = allcfs.crazyflies[0:3] 175 | crazyflies_2= allcfs.crazyflies[3:drone_number] 176 | swarm_3,swarm_4 = swarm2.swarm_separate(crazyflies_1, crazyflies_2) 177 | swarm_3.takeoff_ascyn(hight+1, 10) 178 | swarm_4.takeoff_ascyn(hight-1, 10) 179 | swarm_3.take_formation(distance, 1, hight+1, 0, 0) 180 | swarm_4.take_formation(distance, sep_swarm, hight-1, 0, 0) 181 | swarm_3.navigation_2([-4,-4,hight+1], 10) 182 | swarm_4.navigation_2([4,4,hight-1], 10) 183 | 184 | print("Reunification is taking place....") 185 | timeHelper.sleep(10) 186 | swarm2.take_formation(distance,sep_swarm, hight, 0, 0) 187 | #not working like expected 188 | elif selection == 6 : 189 | point_x = float(input("x : ")) 190 | point_y = float(input("y : ")) 191 | angle = m.atan((point_y-cur_y)/(point_x-cur_x)) 192 | angle = -angle*180/m.pi 193 | swarm2.rotasyon_3(swarm2.sum_rot+angle, cur_x, cur_y, hight, distance) 194 | swarm2.navigation_2([point_x,point_y,hight], 10) 195 | cur_x = point_x 196 | cur_y = point_y 197 | elif selection == 7: 198 | swarm2.takeoff_scyn(0, 5) 199 | hight = 0 200 | 201 | 202 | 203 | timeHelper.sleep(10) 204 | 205 | swarm2.change_formation(1) 206 | 207 | 208 | timeHelper.sleep(10) 209 | 210 | 211 | 212 | -------------------------------------------------------------------------------- /src/sub_functions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.optimize import linear_sum_assignment 3 | import math as m 4 | 5 | 6 | #theta angle is positive throught to CW. 7 | def transformation_2d(relative_loc,theta,translation,Z): 8 | relative_matrix = relative_loc 9 | relative_matrix[2] = Z 10 | relative_matrix = relative_matrix.transpose() 11 | rotation_matrix = np.array( [[m.cos(theta),-m.sin(theta),translation[0]] , [m.sin(theta),m.cos(theta),translation[1]],[0,0,1]]) 12 | absolute_matrix = np.matmul(rotation_matrix,relative_matrix) 13 | return absolute_matrix 14 | 15 | def inverse_transformation_2d(absolute_loc,theta,translation,Z): 16 | absolute_matrix = absolute_loc 17 | absolute_matrix[2] = Z 18 | 19 | rotation_matrix = np.linalg.inv(np.array( [[m.cos(theta),-m.sin(theta),translation[0]] , [m.sin(theta),m.cos(theta),translation[1]],[0,0,1]])) 20 | relative_matrix = np.matmul(rotation_matrix,absolute_matrix) 21 | 22 | return relative_matrix 23 | 24 | mat = [[1,1,1],[1+m.sqrt(2),1+m.sqrt(2),1],[1+2*m.sqrt(2),1,1],[1+m.sqrt(2),1-m.sqrt(2),1]] 25 | i=0 26 | while i<4: 27 | print(inverse_transformation_2d(mat[i],m.pi/4,[1+m.sqrt(2),1,1], 1)) 28 | i+=1 29 | #dronların mevcut konumlarından, istenilen diğer konumlara minumum maaliyet ile 30 | #gitmesini sağlayan fonskiyonlar. 31 | 32 | def cost_for_move(current_loc,desired_loc): 33 | return m.sqrt( pow(desired_loc[0]-current_loc[0],2) + pow(desired_loc[1]-current_loc[1],2) + pow(desired_loc[2]-current_loc[2],2)) 34 | 35 | def hungarian_assigment(current_locs,desired_locs): #np array ver 36 | cost_matrix = np.zeros(shape=(current_locs.shape[0],current_locs.shape[0])) 37 | i=0 38 | j=0 39 | while i < current_locs.shape[0]: 40 | while j < desired_locs.shape[0]: 41 | cost_matrix[i][j] = cost_for_move(current_locs[i],desired_locs[j]) 42 | j+=1 43 | i+=1 44 | j=0 45 | 46 | row_ind,col_ind = linear_sum_assignment(cost_matrix) 47 | opt_ass = col_ind 48 | drons_is_going = np.zeros((desired_locs.shape[0],desired_locs.shape[1])) 49 | j=0 50 | print("current") 51 | print(current_locs) 52 | print("desired_locs") 53 | print(desired_locs) 54 | while j < current_locs.shape[0]: 55 | drons_is_going[j] = desired_locs[opt_ass[j]] 56 | j+=1 57 | return drons_is_going 58 | 59 | 60 | #a ve b noktaları arasında bir doğru olduğunu düşünelim: 61 | #bu doğru üzerindeki eleman sayısı number_of_particle paremetresiyle alınır 62 | #fonksiyon bize aynı doğru üzerinde number_of_particle+1 adet nokta bulunan 63 | #eşit aralıkların konumlarını verir. 1 boyutlu düşünecek olursak a=0 ,b =6 64 | #ve number_of_particle=0 ise fonksiyon bize 3 noktasını verir . eğer number_of_particle=1 65 | #ise 2 ve 4 noktalarını verir. 66 | def re_construct_line(a_point, b_point, number_of_particle, altitude): 67 | points = np.zeros((number_of_particle + 1, 3)) 68 | i = 0 69 | while i < number_of_particle + 1: 70 | j = i + 1 71 | points[i] = [a_point[0] + (j * ((b_point[0] - a_point[0]) / (number_of_particle + 2))), 72 | a_point[1] + (j * ((b_point[1] - a_point[1]) / (number_of_particle + 2))), altitude] 73 | i += 1 74 | return points 75 | #this for just symetric shapes this is not working exactly etc poligon 76 | def calc_center(points): 77 | x = [] 78 | y = [] 79 | z = points[0][2] 80 | aralar = [] 81 | edges = [] 82 | i=0 83 | while i< len(points): 84 | x.append(points[i][0]) 85 | y.append(points[i][1]) 86 | i+=1 87 | maximum_x = max(x) 88 | minimum_x = min(x) 89 | maximum_y = max(y) 90 | minimum_y = min(y) 91 | i=0 92 | while i