├── tests └── __init__.py ├── examples └── example1.py ├── pyproject.toml ├── README.md ├── LICENSE └── darpy ├── __init__.py └── darp.py /tests/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /examples/example1.py: -------------------------------------------------------------------------------- 1 | from darpy import * 2 | 3 | 4 | problem = DARPProblem( 5 | map=DARPMap(rows=10, columns=10), 6 | agents=[DARPCoordinate(x=1, y=1), DARPCoordinate(x=6, y=5)], 7 | obstacles=[], 8 | ) 9 | 10 | 11 | solved, solution = problem.solve() 12 | 13 | print(f'solved: {solved}') 14 | print(solution.astype(int)) 15 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.poetry] 2 | name = "darpy" 3 | version = "0.1.0" 4 | description = "" 5 | authors = ["user "] 6 | readme = "README.md" 7 | 8 | [tool.poetry.dependencies] 9 | python = "^3.8" 10 | numpy = "^1.24.1" 11 | opencv-python = "^4.7.0.68" 12 | 13 | 14 | [build-system] 15 | requires = ["poetry-core"] 16 | build-backend = "poetry.core.masonry.api" 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # darpy 2 | 3 | A bug-fixed, refactored and repackaged version of [alice-st/DARP](https://github.com/oldworship/DARP-python). This is a user-friendly implementation of the DARP algorithm for multi-agent coverage path planning (MCPP). 4 | 5 | ```sh 6 | pip install git+https://github.com/oelin/darpy 7 | ``` 8 | 9 | 10 | ## Improved API 11 | 12 | ```py 13 | from darpy import DARPCoordinate, DARPMap, DARPProblem 14 | 15 | 16 | problem = DARPProblem( 17 | map = DARPMap( 18 | rows = 10, 19 | columns = 10, 20 | ), 21 | agents = [ 22 | DARPCoordinate(3, 3), 23 | DARPCoordinate(6, 0), 24 | ], 25 | obstacles = [ 26 | DARPCoordinate(6, 4), 27 | DARPCoordinate(5, 5), 28 | DARPCoordinate(5, 6), 29 | DARPCoordinate(5, 4), 30 | ], 31 | ) 32 | 33 | 34 | solved, solution = problem.solve() 35 | ``` 36 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Oelin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /darpy/__init__.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2023 Oelin 4 | 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | 24 | from typing import List 25 | from dataclasses import dataclass 26 | import numpy as np 27 | 28 | from . import darp 29 | 30 | 31 | @dataclass 32 | class DARPMap: 33 | 34 | rows: int = 10 35 | columns: int = 10 36 | 37 | 38 | @dataclass 39 | class DARPCoordinate: 40 | 41 | x: int = 0 42 | y: int = 0 43 | 44 | 45 | def to_index(self, map: DARPMap): 46 | return self.y * map.rows + self.x 47 | 48 | 49 | @dataclass 50 | class DARPProblem: 51 | 52 | map: DARPMap 53 | agents: List[DARPCoordinate] 54 | obstacles: List[DARPCoordinate] 55 | 56 | 57 | def solve(self, iterations: int = 5000): 58 | 59 | # Compute agent and obstacle indices... 60 | 61 | agent_indices = [agent.to_index(self.map) for agent in self.agents] 62 | obstacle_indices = [obstacle.to_index(self.map) for obstacle in self.obstacles] 63 | 64 | 65 | # Compute even portions... 66 | 67 | portions = np.ones(len(self.agents)) / len(self.agents) 68 | 69 | 70 | # Solve the problem... 71 | 72 | solver = darp.DARP( 73 | nx=self.map.columns, 74 | ny=self.map.rows, 75 | notEqualPortions=False, 76 | given_initial_positions=agent_indices, 77 | given_portions=portions, 78 | obstacles_positions=obstacle_indices, 79 | visualization=False, 80 | MaxIter=iterations, 81 | ) 82 | 83 | try: 84 | self.solved,_ = solver.divideRegions() 85 | self.solution = solver.BinaryRobotRegions 86 | 87 | return self.solved, self.solution 88 | 89 | except: 90 | return False, None 91 | -------------------------------------------------------------------------------- /darpy/darp.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import numpy as np 3 | import cv2 4 | 5 | def assign(droneNo, rows, cols, GridEnv, MetricMatrix, A): 6 | 7 | ArrayOfElements = np.zeros(droneNo) 8 | for i in range(rows): 9 | for j in range(cols): 10 | if GridEnv[i, j] == -1: 11 | minV = MetricMatrix[0, i, j] 12 | indMin = 0 13 | for r in range(droneNo): 14 | if MetricMatrix[r, i, j] < minV: 15 | minV = MetricMatrix[r, i, j] 16 | indMin = r 17 | 18 | A[i, j] = indMin 19 | ArrayOfElements[indMin] += 1 20 | 21 | elif GridEnv[i, j] == -2: 22 | A[i, j] = droneNo 23 | return A, ArrayOfElements 24 | 25 | 26 | def inverse_binary_map_as_uint8(BinaryMap): 27 | # cv2.distanceTransform needs input of dtype unit8 (8bit) 28 | return np.logical_not(BinaryMap).astype(np.uint8) 29 | 30 | 31 | def euclidian_distance_points2d(array1: np.array, array2: np.array) -> np.float_: 32 | # this runs much faster than the (numba) np.linalg.norm and is totally enough for our purpose 33 | return ( 34 | ((array1[0] - array2[0]) ** 2) + 35 | ((array1[1] - array2[1]) ** 2) 36 | ) ** 0.5 37 | 38 | 39 | def constructBinaryImages(labels_im, robo_start_point, rows, cols): 40 | BinaryRobot = np.copy(labels_im) 41 | BinaryNonRobot = np.copy(labels_im) 42 | for i in range(rows): 43 | for j in range(cols): 44 | if labels_im[i, j] == labels_im[robo_start_point]: 45 | BinaryRobot[i, j] = 1 46 | BinaryNonRobot[i, j] = 0 47 | elif labels_im[i, j] != 0: 48 | BinaryRobot[i, j] = 0 49 | BinaryNonRobot[i, j] = 1 50 | 51 | return BinaryRobot, BinaryNonRobot 52 | 53 | 54 | def CalcConnectedMultiplier(rows, cols, dist1, dist2, CCvariation): 55 | returnM = np.zeros((rows, cols)) 56 | MaxV = 0 57 | MinV = 2**30 58 | 59 | for i in range(rows): 60 | for j in range(cols): 61 | returnM[i, j] = dist1[i, j] - dist2[i, j] 62 | if MaxV < returnM[i, j]: 63 | MaxV = returnM[i, j] 64 | if MinV > returnM[i, j]: 65 | MinV = returnM[i, j] 66 | 67 | for i in range(rows): 68 | for j in range(cols): 69 | returnM[i, j] = (returnM[i, j]-MinV)*((2*CCvariation)/(MaxV - MinV)) + (1-CCvariation) 70 | 71 | return returnM 72 | 73 | 74 | class DARP: 75 | def __init__(self, nx, ny, notEqualPortions, given_initial_positions, given_portions, obstacles_positions, 76 | visualization, MaxIter=80000, CCvariation=0.01, 77 | randomLevel=0.0001, dcells=2, 78 | importance=False): 79 | 80 | self.rows = nx 81 | self.cols = ny 82 | self.initial_positions, self.obstacles_positions, self.portions = self.sanity_check(given_initial_positions, given_portions, obstacles_positions, notEqualPortions) 83 | 84 | #self.initial_positions = given_initial_positions 85 | #self.obstacles_positions = obstacles_positions 86 | #self.portions = given_portions 87 | 88 | self.visualization = visualization 89 | self.MaxIter = MaxIter 90 | self.CCvariation = CCvariation 91 | self.randomLevel = randomLevel 92 | self.dcells = dcells 93 | self.importance = importance 94 | self.notEqualPortions = notEqualPortions 95 | 96 | 97 | #print("\nInitial Conditions Defined:") 98 | #print("Grid Dimensions:", nx, ny) 99 | #print("Number of Robots:", len(self.initial_positions)) 100 | #print("Initial Robots' positions", self.initial_positions) 101 | #print("Portions for each Robot:", self.portions, "\n") 102 | 103 | self.droneNo = len(self.initial_positions) 104 | self.A = np.zeros((self.rows, self.cols)) 105 | self.GridEnv = self.defineGridEnv() 106 | 107 | self.connectivity = np.zeros((self.droneNo, self.rows, self.cols), dtype=np.uint8) 108 | self.BinaryRobotRegions = np.zeros((self.droneNo, self.rows, self.cols), dtype=bool) 109 | 110 | self.MetricMatrix, self.termThr, self.Notiles, self.DesireableAssign, self.TilesImportance, self.MinimumImportance, self.MaximumImportance= self.construct_Assignment_Matrix() 111 | self.ArrayOfElements = np.zeros(self.droneNo) 112 | self.color = [] 113 | 114 | for r in range(self.droneNo): 115 | np.random.seed(r) 116 | self.color.append(list(np.random.choice(range(256), size=3))) 117 | 118 | np.random.seed(1) 119 | if self.visualization: 120 | self.assignment_matrix_visualization = darp_area_visualization(self.A, self.droneNo, self.color, self.initial_positions) 121 | 122 | def sanity_check(self, given_initial_positions, given_portions, obs_pos, notEqualPortions): 123 | initial_positions = [] 124 | for position in given_initial_positions: 125 | if position < 0 or position >= self.rows * self.cols: 126 | #print("Initial positions should be inside the Grid.") 127 | sys.exit(1) 128 | initial_positions.append((position // self.cols, position % self.cols)) 129 | 130 | obstacles_positions = [] 131 | for obstacle in obs_pos: 132 | if obstacle < 0 or obstacle >= self.rows * self.cols: 133 | #print("Obstacles should be inside the Grid.") 134 | sys.exit(2) 135 | obstacles_positions.append((obstacle // self.cols, obstacle % self.cols)) 136 | 137 | portions = [] 138 | if notEqualPortions: 139 | portions = given_portions 140 | else: 141 | for drone in range(len(initial_positions)): 142 | portions.append(1 / len(initial_positions)) 143 | 144 | if len(initial_positions) != len(portions): 145 | #print("Portions should be defined for each drone") 146 | sys.exit(3) 147 | 148 | s = sum(portions) 149 | if abs(s - 1) >= 0.0001: 150 | #print("Sum of portions should be equal to 1.") 151 | sys.exit(4) 152 | 153 | for position in initial_positions: 154 | for obstacle in obstacles_positions: 155 | if position[0] == obstacle[0] and position[1] == obstacle[1]: 156 | #print("Initial positions should not be on obstacles") 157 | sys.exit(5) 158 | 159 | return initial_positions, obstacles_positions, portions 160 | 161 | def defineGridEnv(self): 162 | GridEnv = np.full(shape=(self.rows, self.cols), fill_value=-1) # create non obstacle map with value -1 163 | 164 | # obstacle tiles value is -2 165 | for idx, obstacle_pos in enumerate(self.obstacles_positions): 166 | GridEnv[obstacle_pos[0], obstacle_pos[1]] = -2 167 | 168 | connectivity = np.zeros((self.rows, self.cols)) 169 | 170 | mask = np.where(GridEnv == -1) 171 | connectivity[mask[0], mask[1]] = 255 172 | image = np.uint8(connectivity) 173 | num_labels, labels_im = cv2.connectedComponents(image, connectivity=4) 174 | 175 | if num_labels > 2: 176 | #print("The environment grid MUST not have unreachable and/or closed shape regions") 177 | sys.exit(6) 178 | 179 | # initial robot tiles will have their array.index as value 180 | for idx, robot in enumerate(self.initial_positions): 181 | GridEnv[robot] = idx 182 | self.A[robot] = idx 183 | 184 | return GridEnv 185 | 186 | def divideRegions(self): 187 | success = False 188 | cancelled = False 189 | criterionMatrix = np.zeros((self.rows, self.cols)) 190 | iteration = 0 191 | 192 | while self.termThr <= self.dcells and not success and not cancelled: 193 | downThres = (self.Notiles - self.termThr*(self.droneNo-1))/(self.Notiles*self.droneNo) 194 | upperThres = (self.Notiles + self.termThr)/(self.Notiles*self.droneNo) 195 | 196 | success = True 197 | 198 | # Main optimization loop 199 | 200 | iteration=0 201 | 202 | while iteration <= self.MaxIter and not cancelled: 203 | self.A, self.ArrayOfElements = assign(self.droneNo, 204 | self.rows, 205 | self.cols, 206 | self.GridEnv, 207 | self.MetricMatrix, 208 | self.A) 209 | ConnectedMultiplierList = np.ones((self.droneNo, self.rows, self.cols)) 210 | ConnectedRobotRegions = np.zeros(self.droneNo) 211 | plainErrors = np.zeros((self.droneNo)) 212 | divFairError = np.zeros((self.droneNo)) 213 | 214 | self.update_connectivity() 215 | for r in range(self.droneNo): 216 | ConnectedMultiplier = np.ones((self.rows, self.cols)) 217 | ConnectedRobotRegions[r] = True 218 | num_labels, labels_im = cv2.connectedComponents(self.connectivity[r, :, :], connectivity=4) 219 | if num_labels > 2: 220 | ConnectedRobotRegions[r] = False 221 | BinaryRobot, BinaryNonRobot = constructBinaryImages(labels_im, self.initial_positions[r], self.rows, self.cols) 222 | ConnectedMultiplier = CalcConnectedMultiplier(self.rows, self.cols, 223 | self.NormalizedEuclideanDistanceBinary(True, BinaryRobot), 224 | self.NormalizedEuclideanDistanceBinary(False, BinaryNonRobot), self.CCvariation) 225 | ConnectedMultiplierList[r, :, :] = ConnectedMultiplier 226 | plainErrors[r] = self.ArrayOfElements[r]/(self.DesireableAssign[r]*self.droneNo) 227 | if plainErrors[r] < downThres: 228 | divFairError[r] = downThres - plainErrors[r] 229 | elif plainErrors[r] > upperThres: 230 | divFairError[r] = upperThres - plainErrors[r] 231 | 232 | if self.IsThisAGoalState(self.termThr, ConnectedRobotRegions): 233 | break 234 | 235 | TotalNegPerc = 0 236 | totalNegPlainErrors = 0 237 | correctionMult = np.zeros(self.droneNo) 238 | 239 | for r in range(self.droneNo): 240 | if divFairError[r] < 0: 241 | TotalNegPerc += np.absolute(divFairError[r]) 242 | totalNegPlainErrors += plainErrors[r] 243 | 244 | correctionMult[r] = 1 245 | 246 | for r in range(self.droneNo): 247 | if totalNegPlainErrors != 0: 248 | if divFairError[r] < 0: 249 | correctionMult[r] = 1 + (plainErrors[r]/totalNegPlainErrors)*(TotalNegPerc/2) 250 | else: 251 | correctionMult[r] = 1 - (plainErrors[r]/totalNegPlainErrors)*(TotalNegPerc/2) 252 | 253 | criterionMatrix = self.calculateCriterionMatrix( 254 | self.TilesImportance[r], 255 | self.MinimumImportance[r], 256 | self.MaximumImportance[r], 257 | correctionMult[r], 258 | divFairError[r] < 0) 259 | 260 | self.MetricMatrix[r] = self.FinalUpdateOnMetricMatrix( 261 | criterionMatrix, 262 | self.generateRandomMatrix(), 263 | self.MetricMatrix[r], 264 | ConnectedMultiplierList[r, :, :]) 265 | 266 | iteration += 1 267 | if self.visualization: 268 | self.assignment_matrix_visualization.placeCells(self.A, iteration_number=iteration) 269 | time.sleep(0.2) 270 | 271 | if iteration >= self.MaxIter: 272 | self.MaxIter = self.MaxIter/2 273 | success = False 274 | self.termThr += 1 275 | 276 | self.getBinaryRobotRegions() 277 | return success, iteration 278 | 279 | def getBinaryRobotRegions(self): 280 | ind = np.where(self.A < self.droneNo) 281 | temp = (self.A[ind].astype(int),)+ind 282 | self.BinaryRobotRegions[temp] = True 283 | 284 | def generateRandomMatrix(self): 285 | RandomMatrix = np.zeros((self.rows, self.cols)) 286 | RandomMatrix = 2*self.randomLevel*np.random.uniform(0, 1,size=RandomMatrix.shape) + (1 - self.randomLevel) 287 | return RandomMatrix 288 | 289 | def FinalUpdateOnMetricMatrix(self, CM, RM, currentOne, CC): 290 | MMnew = np.zeros((self.rows, self.cols)) 291 | MMnew = currentOne*CM*RM*CC 292 | 293 | return MMnew 294 | 295 | def IsThisAGoalState(self, thresh, connectedRobotRegions): 296 | for r in range(self.droneNo): 297 | if np.absolute(self.DesireableAssign[r] - self.ArrayOfElements[r]) > thresh or not connectedRobotRegions[r]: 298 | return False 299 | return True 300 | 301 | def update_connectivity(self): 302 | self.connectivity = np.zeros((self.droneNo, self.rows, self.cols), dtype=np.uint8) 303 | for i in range(self.droneNo): 304 | mask = np.where(self.A == i) 305 | self.connectivity[i, mask[0], mask[1]] = 255 306 | 307 | # Construct Assignment Matrix 308 | def construct_Assignment_Matrix(self): 309 | Notiles = self.rows*self.cols 310 | fair_division = 1/self.droneNo 311 | effectiveSize = Notiles - self.droneNo - len(self.obstacles_positions) 312 | termThr = 0 313 | 314 | if effectiveSize % self.droneNo != 0: 315 | termThr = 1 316 | 317 | DesireableAssign = np.zeros(self.droneNo) 318 | MaximunDist = np.zeros(self.droneNo) 319 | MaximumImportance = np.zeros(self.droneNo) 320 | MinimumImportance = np.zeros(self.droneNo) 321 | 322 | for i in range(self.droneNo): 323 | DesireableAssign[i] = effectiveSize * self.portions[i] 324 | MinimumImportance[i] = sys.float_info.max 325 | if (DesireableAssign[i] != int(DesireableAssign[i]) and termThr != 1): 326 | termThr = 1 327 | 328 | AllDistances = np.zeros((self.droneNo, self.rows, self.cols)) 329 | TilesImportance = np.zeros((self.droneNo, self.rows, self.cols)) 330 | 331 | for x in range(self.rows): 332 | for y in range(self.cols): 333 | tempSum = 0 334 | for r in range(self.droneNo): 335 | AllDistances[r, x, y] = euclidian_distance_points2d(np.array(self.initial_positions[r]), np.array((x, y))) # E! 336 | if AllDistances[r, x, y] > MaximunDist[r]: 337 | MaximunDist[r] = AllDistances[r, x, y] 338 | tempSum += AllDistances[r, x, y] 339 | 340 | for r in range(self.droneNo): 341 | if tempSum - AllDistances[r, x, y] != 0: 342 | TilesImportance[r, x, y] = 1/(tempSum - AllDistances[r, x, y]) 343 | else: 344 | TilesImportance[r, x, y] = 1 345 | # Todo FixMe! 346 | if TilesImportance[r, x, y] > MaximumImportance[r]: 347 | MaximumImportance[r] = TilesImportance[r, x, y] 348 | 349 | if TilesImportance[r, x, y] < MinimumImportance[r]: 350 | MinimumImportance[r] = TilesImportance[r, x, y] 351 | 352 | return AllDistances, termThr, Notiles, DesireableAssign, TilesImportance, MinimumImportance, MaximumImportance 353 | 354 | def calculateCriterionMatrix(self, TilesImportance, MinimumImportance, MaximumImportance, correctionMult, smallerthan_zero,): 355 | returnCrit = np.zeros((self.rows, self.cols)) 356 | if self.importance: 357 | if smallerthan_zero: 358 | returnCrit = (TilesImportance- MinimumImportance)*((correctionMult-1)/(MaximumImportance-MinimumImportance)) + 1 359 | else: 360 | returnCrit = (TilesImportance- MinimumImportance)*((1-correctionMult)/(MaximumImportance-MinimumImportance)) + correctionMult 361 | else: 362 | returnCrit[:, :] = correctionMult 363 | 364 | return returnCrit 365 | 366 | def NormalizedEuclideanDistanceBinary(self, RobotR, BinaryMap): 367 | distRobot = cv2.distanceTransform(inverse_binary_map_as_uint8(BinaryMap), distanceType=2, maskSize=0, dstType=5) 368 | MaxV = np.max(distRobot) 369 | MinV = np.min(distRobot) 370 | 371 | #Normalization 372 | if RobotR: 373 | distRobot = (distRobot - MinV)*(1/(MaxV-MinV)) + 1 374 | else: 375 | distRobot = (distRobot - MinV)*(1/(MaxV-MinV)) 376 | 377 | return distRobot 378 | --------------------------------------------------------------------------------