├── .gitignore ├── requirements.txt └── src ├── __init__.py ├── a_star.py └── main.py /.gitignore: -------------------------------------------------------------------------------- 1 | venv 2 | deliverables 3 | *.idea 4 | src/__pycache__/ 5 | *.pdf 6 | *.png 7 | *.jpg -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | attrs==20.3.0 2 | bumpversion==0.5.3 3 | certifi==2020.12.5 4 | cffi==1.14.5 5 | chardet==4.0.0 6 | checksumdir==1.2.0 7 | click==7.1.2 8 | click-plugins==1.1.1 9 | cligj==0.7.1 10 | colorlog==4.7.2 11 | cryptography==3.4.6 12 | cycler==0.10.0 13 | decorator==4.4.2 14 | dynaconf==3.0.0 15 | elkai==0.1.2 16 | Fiona==1.8.18 17 | GeoAlchemy2==0.6.3 18 | geojson==2.5.0 19 | geopandas==0.7.0 20 | gitchangelog==3.0.4 21 | idna==2.10 22 | invoke==1.5.0 23 | joblib==1.0.1 24 | kiwisolver==1.3.1 25 | lgblkb-tools==2.0.21 26 | matplotlib==3.3.4 27 | more-itertools==8.7.0 28 | munch==2.5.0 29 | networkx==2.5 30 | numpy==1.19.5 31 | opencv-python==4.5.1.48 32 | ortools==7.8.7959 33 | pandas==1.1.5 34 | Pillow==8.1.0 35 | protobuf==3.15.3 36 | pycparser==2.20 37 | pyparsing==2.4.7 38 | pyproj==2.6.1.post1 39 | python-box==5.3.0 40 | python-dateutil==2.8.1 41 | python-log-indenter==0.9 42 | python-telegram-bot==12.8 43 | pytz==2021.1 44 | PyYAML==5.4.1 45 | requests==2.25.1 46 | ruamel.yaml==0.16.12 47 | ruamel.yaml.clib==0.2.2 48 | scikit-learn==0.24.1 49 | scipy==1.5.4 50 | Shapely==1.7.1 51 | six==1.15.0 52 | sortedcontainers==2.3.0 53 | SQLAlchemy==1.3.23 54 | threadpoolctl==2.1.0 55 | toml==0.10.2 56 | tornado==6.1 57 | urllib3==1.26.3 58 | VisiLibity==1.0.10 59 | wrapt==1.12.1 60 | -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Faranio/Path_Planning_Algorithm/2ae3df57439690c00370e75f02229099f47ef118/src/__init__.py -------------------------------------------------------------------------------- /src/a_star.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from src.main import config 4 | 5 | 6 | class Graph: 7 | def __init__(self, graph_dict=None): 8 | self.graph_dict = graph_dict or {} 9 | 10 | def connect(self, A, B, distance=1): 11 | self.graph_dict.setdefault(A, {})[B] = distance 12 | self.graph_dict.setdefault(B, {})[A] = distance 13 | 14 | def get_neighbors(self, a, b=None): 15 | links = self.graph_dict.setdefault(a, {}) 16 | return links.get(b) if b else links 17 | 18 | def get_nodes(self): 19 | s1 = set([k for k in self.graph_dict.keys()]) 20 | s2 = set([k2 for v in self.graph_dict.values() for k2, v2 in v.items()]) 21 | nodes = s1.union(s2) 22 | return list(nodes) 23 | 24 | 25 | class Node: 26 | def __init__(self, name: str, parent=None): 27 | self.name = name 28 | self.parent = parent 29 | self.distance_to_start_node = 0 30 | self.distance_to_end_node = 0 31 | self.total_distance = 0 32 | 33 | def __eq__(self, other): 34 | return self.name == other.name 35 | 36 | def __lt__(self, other): 37 | return self.total_distance < other.total_distance 38 | 39 | def __repr__(self): 40 | return '({0}, {1})'.format(self.name, self.total_distance) 41 | 42 | 43 | def add_to_open(opened_nodes, neighbor): 44 | for node in opened_nodes: 45 | if neighbor == node and neighbor.total_distance > node.total_distance: 46 | return False 47 | return True 48 | 49 | 50 | def update_neighbor_distances(neighbor, node, graph, heuristics): 51 | neighbor.distance_to_start_node = node.distance_to_start_node + graph.get_neighbors(node.name, 52 | neighbor.name) 53 | neighbor.distance_to_end_node = heuristics.get(neighbor.name) 54 | neighbor.total_distance = neighbor.distance_to_start_node + neighbor.distance_to_end_node 55 | return neighbor 56 | 57 | 58 | def update_neighbors_distances(neighbors, node, opened, closed, graph, heuristics): 59 | for key, value in neighbors.items(): 60 | neighbor = Node(key, node) 61 | 62 | if neighbor in closed: 63 | continue 64 | 65 | neighbor = update_neighbor_distances(neighbor, node, graph, heuristics) 66 | 67 | if add_to_open(opened, neighbor): 68 | opened.append(neighbor) 69 | 70 | return opened, closed 71 | 72 | 73 | def construct_path(current_node, start_node): 74 | path = [] 75 | 76 | while current_node != start_node: 77 | path.append([current_node.name, str(current_node.distance_to_start_node)]) 78 | current_node = current_node.parent 79 | 80 | path.append([start_node.name, str(start_node.distance_to_start_node)]) 81 | return path[::-1] 82 | 83 | 84 | def a_star_search(graph, heuristics, start, end): 85 | opened = [] 86 | closed = [] 87 | 88 | start_node = Node(start) 89 | goal_node = Node(end) 90 | 91 | opened.append(start_node) 92 | 93 | while len(opened) > 0: 94 | opened.sort() 95 | current_node = opened.pop(0) 96 | closed.append(current_node) 97 | 98 | if current_node == goal_node: 99 | return construct_path(current_node, start_node) 100 | 101 | neighbors = graph.get_neighbors(current_node.name) 102 | opened, closed = update_neighbors_distances(neighbors, current_node, opened, closed, graph, heuristics) 103 | 104 | return None 105 | 106 | 107 | def find_a_star_path(distance_matrix, mapping, start, end): 108 | graph = Graph() 109 | heuristics = {} 110 | 111 | rows, cols = np.where(distance_matrix < config['DEFAULT_EDGE_COST']) 112 | 113 | for row, col in zip(rows, cols): 114 | point1 = mapping[row] 115 | point2 = mapping[col] 116 | graph.connect(str(point1), str(point2), distance_matrix[row][col]) 117 | 118 | for row in rows: 119 | point = mapping[row] 120 | heuristics[str(point)] = 0 121 | 122 | path = a_star_search(graph, heuristics, str(start), str(end)) 123 | return path 124 | -------------------------------------------------------------------------------- /src/main.py: -------------------------------------------------------------------------------- 1 | import collections 2 | import itertools 3 | import matplotlib.pyplot as plt 4 | import more_itertools as mi 5 | import shapely.geometry as shg 6 | 7 | from lgblkb_tools import logger 8 | from lgblkb_tools.common.utils import ParallelTasker 9 | from lgblkb_tools.geometry import FieldPoly 10 | from tsp_solver.greedy import solve_tsp 11 | 12 | from src.a_star import * 13 | 14 | config = { 15 | 'DEFAULT_EDGE_COST': 1e6, 16 | 'DFS_THRESHOLD': 45, 17 | 'EXISTING_EDGE_COST': 1, 18 | 'EXTRAPOLATION_OFFSET': 2, 19 | 'MINIMUM_DISTANCE': 1e-6, 20 | 'PATH_WIDTH': 40, 21 | 'REQUIRED_EDGE_COST': -1e5, 22 | 'SEARCH_LOOP_LIMIT': 3e5, 23 | 'WORKERS_COUNT': 4 24 | } 25 | 26 | 27 | def get_field_lines(field_poly: FieldPoly, show=False): 28 | field_poly = FieldPoly.as_valid(field_poly) 29 | field_lines = field_poly.get_field_lines_old(config['PATH_WIDTH'], 30 | field_poly.get_min_cost(config['PATH_WIDTH']).base_line, show=show) 31 | return field_lines 32 | 33 | 34 | def get_paths(poly: FieldPoly, start, goal, loop_limit=5e6): 35 | graph = poly.adjacency_info 36 | 37 | if len(poly.G.decision_points['all']) < config['DFS_THRESHOLD']: 38 | pop_index = -1 39 | loop_limit = 5e6 40 | else: 41 | pop_index = 0 42 | 43 | hole_points = [hole.geometry.representative_point() for hole in poly.holes] 44 | stack = [(start, [start])] 45 | seen = set() 46 | loop_counter = itertools.count() 47 | 48 | while stack: 49 | (vertex, path) = stack.pop(pop_index) 50 | curr_loop_count = next(loop_counter) 51 | 52 | if curr_loop_count % int(loop_limit / 50) == 0: 53 | logger.debug(f"curr_loop_count: {curr_loop_count}") 54 | 55 | for _next in graph[vertex] - set(path): 56 | if _next == goal: 57 | some_path = path + [_next] 58 | 59 | if len(some_path) < 3: 60 | continue 61 | 62 | path_hash = sum(hash(x) for x in some_path) 63 | 64 | if path_hash in seen: 65 | continue 66 | 67 | seen.add(path_hash) 68 | some_field = FieldPoly(some_path) 69 | 70 | if some_field.geometry.area < config['MINIMUM_DISTANCE']: 71 | continue 72 | 73 | if not some_field.geometry.is_valid: 74 | continue 75 | 76 | some_field = FieldPoly.as_valid(some_field.geometry) 77 | contains_hole = False 78 | 79 | for point_in_hole in hole_points: 80 | if point_in_hole.within(some_field.geometry): 81 | contains_hole = True 82 | break 83 | 84 | if contains_hole: 85 | continue 86 | 87 | yield some_field 88 | else: 89 | stack.append((_next, path + [_next])) 90 | 91 | if curr_loop_count > loop_limit: 92 | logger.info(f"Final_loop_count: {curr_loop_count}") 93 | break 94 | 95 | 96 | def get_other_polys(parent_poly, child_poly): 97 | try: 98 | diff_result = parent_poly.geometry.difference(child_poly.geometry) 99 | except Exception as exc: 100 | logger.warning(str(exc)) 101 | return [] 102 | 103 | if isinstance(diff_result, shg.Polygon): 104 | polygons = [diff_result] 105 | elif isinstance(diff_result, shg.MultiPolygon): 106 | polygons = [x for x in diff_result] 107 | elif isinstance(diff_result, shg.GeometryCollection): 108 | diff_results = diff_result 109 | polygons = list() 110 | 111 | for diff_result in diff_results: 112 | if isinstance(diff_result, shg.Polygon): 113 | polygons.append(diff_result) 114 | elif isinstance(diff_result, shg.MultiPolygon): 115 | polygons.extend([x for x in diff_result]) 116 | else: 117 | logger.error(f"diff_result: \n{diff_result}") 118 | raise NotImplementedError(str(type(diff_result))) 119 | 120 | polygons = [FieldPoly.as_valid(x) for x in polygons if x.area > config['MINIMUM_DISTANCE']] 121 | return polygons 122 | 123 | 124 | Candidate = collections.namedtuple('Candidate', ['acr', 'field']) 125 | 126 | 127 | def decompose_from_point(field_poly, some_point, show=False): 128 | counter = itertools.count() 129 | curr_count = 0 130 | start = some_point 131 | goal = field_poly.G.nodes[some_point]['lines'][0][-1] 132 | best = Candidate(field_poly.area_cost_ratio, field_poly) 133 | 134 | if show: 135 | field_poly.plot() 136 | logger.debug(f"Best: {best}") 137 | best.field.plot(text=f'Accuracy = {best.acr}') 138 | start.plot('Start') 139 | goal.plot('Goal') 140 | plt.tight_layout() 141 | plt.show() 142 | 143 | decomposed_polygons = [best.field] 144 | 145 | for path_field in get_paths(field_poly, start, goal, loop_limit=config['SEARCH_LOOP_LIMIT']): 146 | curr_count = next(counter) 147 | polygons = get_other_polys(field_poly, path_field) 148 | costs = [p.get_min_cost(config['PATH_WIDTH']).cost for p in [path_field] + polygons] 149 | area_cost_ratio = field_poly.geometry.area / sum(costs) 150 | 151 | if area_cost_ratio > best.acr: 152 | best = Candidate(area_cost_ratio, path_field) 153 | decomposed_polygons = [best.field, *polygons] 154 | 155 | logger.debug(f"Final_count: {curr_count}") 156 | logger.info(f"Final best: {best}") 157 | 158 | if show: 159 | field_poly.plot() 160 | best.field.plot(text=f'Accuracy = {best.acr}') 161 | 162 | return decomposed_polygons 163 | 164 | 165 | def decompose_from_points(field_poly: FieldPoly, points=None, use_mp=False, show=False): 166 | resultant_polys = [field_poly] 167 | max_acr = field_poly.area_cost_ratio 168 | logger.debug(f"Starting area_cost_ratio: {max_acr}") 169 | points = field_poly.get_outer_points() if points is None else points 170 | 171 | if use_mp: 172 | chunks_of_polygons = ParallelTasker(decompose_from_point, field_poly, show=show)\ 173 | .set_run_params(some_point=points).run(workers_count=config['WORKERS_COUNT']) 174 | 175 | for res_polygons in chunks_of_polygons: 176 | decomp_cost = res_polygons[0].area_cost_ratio 177 | 178 | if decomp_cost > max_acr: 179 | logger.info(f"Better decomposition accuracy found: {decomp_cost}") 180 | max_acr = decomp_cost 181 | resultant_polys = res_polygons 182 | else: 183 | for outer_point in points: 184 | res_polygons = decompose_from_point(field_poly, outer_point, show=show) 185 | decomp_cost = res_polygons[0].area_cost_ratio 186 | 187 | if decomp_cost > max_acr: 188 | logger.info(f"Better decomposition accuracy found: {decomp_cost}") 189 | max_acr = decomp_cost 190 | resultant_polys = res_polygons 191 | 192 | return resultant_polys 193 | 194 | 195 | @logger.trace() 196 | def perform_optimization(field_poly, use_mp=False): 197 | polygons = decompose_from_points(field_poly, field_poly.get_outer_points(), use_mp=use_mp, show=False) 198 | optimum_polygons = [polygons[0]] 199 | 200 | if use_mp: 201 | if not len(polygons[1:]) == 0: 202 | chunks_of_optimum_sub_polygons = mi.flatten(ParallelTasker(perform_optimization) 203 | .set_run_params(field_poly=polygons[1:]).run(len(polygons[1:]))) 204 | optimum_polygons.extend(chunks_of_optimum_sub_polygons) 205 | else: 206 | for other_polygon in polygons[1:]: 207 | optimum_sub_polygons = perform_optimization(other_polygon) 208 | optimum_polygons.extend(optimum_sub_polygons) 209 | 210 | return optimum_polygons 211 | 212 | 213 | @logger.trace() 214 | def plot_optimum_polygons(optimum_polygons, field_poly): 215 | plt.figure(figsize=(20, 20)) 216 | costs = [] 217 | total_field_count = 0 218 | all_lines = [] 219 | 220 | for optimum_polygon in optimum_polygons: 221 | lines_count = len(get_field_lines(optimum_polygon, show=False)) 222 | total_field_count += lines_count 223 | optimum_polygon.plot(lw=5) 224 | temp_x, temp_y = optimum_polygon.polygon.centroid.xy 225 | temp_x, temp_y = float(temp_x[0]), float(temp_y[0]) 226 | plt.text(temp_x, temp_y, f"Cost: {lines_count}", ha='center', va='center', fontsize=40) 227 | costs.append(optimum_polygon.get_min_cost(config['PATH_WIDTH']).cost) 228 | field_lines = get_field_lines(optimum_polygon) 229 | 230 | for line in field_lines: 231 | ls = shg.LineString(line.line) 232 | all_lines.append(ls) 233 | ls_x, ls_y = ls.xy 234 | plt.plot(ls_x, ls_y, c='r', lw=5) 235 | 236 | final_acr = field_poly.geometry.area / sum(costs) 237 | logger.info(f"Final Area Cost Ratio: {final_acr}") 238 | logger.info(f"Total Field Count: {total_field_count}") 239 | field_poly.plot(lw=5) 240 | plt.gca().set_aspect('equal', 'box') 241 | plt.grid(axis='both') 242 | plt.title("Optimum Polygons", fontsize=50) 243 | plt.xticks(fontsize=30) 244 | plt.yticks(fontsize=30) 245 | plt.tight_layout() 246 | plt.show() 247 | 248 | 249 | def get_lines(field_poly): 250 | all_lines = [] 251 | 252 | if isinstance(field_poly, list): 253 | for optimum_polygon in field_poly: 254 | field_lines = get_field_lines(optimum_polygon) 255 | 256 | for line in field_lines: 257 | ls = shg.LineString(line.line) 258 | all_lines.append(ls) 259 | else: 260 | field_lines = get_field_lines(field_poly) 261 | 262 | for line in field_lines: 263 | ls = shg.LineString(line.line) 264 | all_lines.append(ls) 265 | 266 | return all_lines 267 | 268 | 269 | def get_extrapolated_line(p1, p2): 270 | divisor = p2[0] - p1[0] 271 | 272 | if divisor != 0: 273 | m = (p2[1] - p1[1]) / (p2[0] - p1[0]) 274 | c = p2[1] - m * p2[0] 275 | sign = 1 if p1[0] - p2[0] > 0 else -1 276 | a = (p1[0] + sign * config['EXTRAPOLATION_OFFSET'], m * (p1[0] + sign * config['EXTRAPOLATION_OFFSET']) + c) 277 | b = (p2[0] + (-1) * sign * config['EXTRAPOLATION_OFFSET'], m * (p2[0] + (-1) * sign * config['EXTRAPOLATION_OFFSET']) + c) 278 | else: 279 | sign = 1 if p1[1] - p2[1] > 0 else -1 280 | a = (p1[0], p1[1] + sign * config['EXTRAPOLATION_OFFSET']) 281 | b = (p2[0], p2[1] + (-1) * sign * config['EXTRAPOLATION_OFFSET']) 282 | 283 | return shg.LineString([a, b]) 284 | 285 | 286 | def get_intersections(ls, E): 287 | intersection_points = set() 288 | unique_points = set() 289 | 290 | for ext_line in E: 291 | ext_ls = shg.LineString(ext_line) 292 | temp = ls.intersection(ext_ls) 293 | 294 | if not temp.is_empty: 295 | res = (round(temp.coords[0][0]), round(temp.coords[0][1])) 296 | 297 | if res not in unique_points: 298 | intersection_points.add(temp.coords[0]) 299 | unique_points.add(res) 300 | 301 | return intersection_points 302 | 303 | 304 | def segments(curve): 305 | return list(map(shg.LineString, zip(curve.coords[:-1], curve.coords[1:]))) 306 | 307 | 308 | def polygons_to_graph(optimum_polygons, distance_threshold=1e-4, show=False): 309 | V, E, R = set(), set(), set() 310 | exterior_lines = set() 311 | optimum_polygon_lines = get_lines(optimum_polygons) 312 | 313 | for polygon in optimum_polygons: 314 | for line in polygon.exterior_lines: 315 | ls = shg.LineString(line) 316 | p1 = ls.coords[0] 317 | p2 = ls.coords[1] 318 | exterior_lines.add((p1, p2)) 319 | V.add(p1) 320 | V.add(p2) 321 | 322 | for line in optimum_polygon_lines: 323 | ls = shg.LineString(line) 324 | p1 = ls.coords[0] 325 | p2 = ls.coords[1] 326 | ls = get_extrapolated_line(p1, p2) 327 | intersections = get_intersections(ls, exterior_lines) 328 | points = [] 329 | 330 | for i in intersections: 331 | points.append(i) 332 | 333 | if len(points) > 0: 334 | V.add(points[0]) 335 | V.add(points[1]) 336 | R.add((points[0], points[1])) 337 | R.add((points[1], points[0])) 338 | E.add((points[0], points[1])) 339 | E.add((points[1], points[0])) 340 | 341 | for ext_edge in exterior_lines: 342 | ext_ls = shg.LineString(ext_edge) 343 | points = [] 344 | for point in V: 345 | p1 = shg.Point(point) 346 | 347 | if ext_ls.distance(p1) < distance_threshold: 348 | points.append(point) 349 | 350 | points = sorted(points) 351 | temp = shg.LineString(points) 352 | temp = segments(temp) 353 | 354 | for line in temp: 355 | p1, p2 = line.coords[0], line.coords[1] 356 | E.add((p1, p2)) 357 | E.add((p2, p1)) 358 | 359 | logger.debug(f"Length of V: {len(V)}") 360 | logger.debug(f"Length of E: {len(E)}") 361 | logger.debug(f"Length of R: {len(R)}") 362 | 363 | if show: 364 | plt.figure(figsize=(20, 20)) 365 | 366 | for edge in E: 367 | plt.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], c='b', lw=5) 368 | 369 | for point in V: 370 | plt.plot(point[0], point[1], marker='o', color='red', markersize=15) 371 | 372 | plt.gca().set_aspect('equal', 'box') 373 | plt.grid(axis='both') 374 | plt.title("Graph Representation", fontsize=50) 375 | plt.xticks(fontsize=30) 376 | plt.yticks(fontsize=30) 377 | plt.tight_layout() 378 | plt.show() 379 | 380 | V, E, R = list(V), list(E), list(R) 381 | 382 | return V, E, R 383 | 384 | 385 | @logger.trace() 386 | def get_distance_matrix(V, E, R): 387 | num_nodes = len(V) 388 | mapping = {} 389 | reverse_mapping = {} 390 | distance_matrix = np.ones([num_nodes, num_nodes]) 391 | 392 | for i in range(len(V)): 393 | mapping[i] = V[i] 394 | reverse_mapping[V[i]] = i 395 | 396 | for i in range(len(distance_matrix)): 397 | for j in range(len(distance_matrix)): 398 | if i != j: 399 | p1 = mapping[i] 400 | p2 = mapping[j] 401 | 402 | if (p1, p2) in R: 403 | distance_matrix[i][j] = config['REQUIRED_EDGE_COST'] 404 | elif (p1, p2) in E: 405 | distance_matrix[i][j] = np.linalg.norm(np.asarray(p2) - np.asarray(p1)) * config['EXISTING_EDGE_COST'] 406 | else: 407 | distance_matrix[i][j] = np.linalg.norm(np.asarray(p2) - np.asarray(p1)) * config['DEFAULT_EDGE_COST'] 408 | 409 | logger.debug(f"Distance Matrix Shape: {distance_matrix.shape}") 410 | return distance_matrix, mapping 411 | 412 | 413 | def add_arrow(line, direction='right', size=30, color=None): 414 | if color is None: 415 | color = line.get_color() 416 | 417 | xdata = line.get_xdata() 418 | ydata = line.get_ydata() 419 | 420 | start_ind = len(xdata) // 2 421 | 422 | if direction == 'right': 423 | end_ind = start_ind + 1 424 | else: 425 | end_ind = start_ind - 1 426 | 427 | line.axes.annotate('', 428 | xytext=(xdata[start_ind], ydata[start_ind]), 429 | xy=(xdata[end_ind], ydata[end_ind]), 430 | arrowprops=dict(arrowstyle="fancy", color=color), 431 | size=size 432 | ) 433 | 434 | 435 | def choose_region(idx, show=True): 436 | if idx == 1: 437 | field_poly = FieldPoly(shg.Polygon([[0, 0], [1000, 0], [1000, 1000], [0, 1000]], holes=[[[200, 200], 438 | [200, 800], 439 | [800, 800], 440 | [800, 200]]])) 441 | elif idx == 2: 442 | field_poly = FieldPoly(shg.Polygon([[0, 0], [400, 0], [400, 400], [600, 400], [600, 0], [1000, 0], [1000, 600], 443 | [800, 700], [750, 800], [1000, 750], [1000, 1000], [0, 1000], [0, 550], 444 | [500, 650], [500, 550], [0, 450]])) 445 | elif idx == 3: 446 | field_poly = FieldPoly( 447 | shg.Polygon([[0, 0], [575, 0], [575, 500], [425, 500], [425, 300], [500, 300], [500, 200], 448 | [300, 200], [300, 650], [700, 650], [700, 0], [1000, 0], [1000, 1000], 449 | [0, 1000]], holes=[[[350, 850], [400, 750], [600, 850], [450, 925]]])) 450 | elif idx == 4: 451 | field_poly = FieldPoly(shg.Polygon([[0, 0], [1000, 0], [1000, 1000], [0, 1000]], holes=[[[100, 400], [200, 300], 452 | [300, 300], [350, 450], 453 | [300, 500], 454 | [150, 450]], 455 | [[750, 300], [750, 100], 456 | [850, 100], 457 | [850, 300]], 458 | [[400, 700], [850, 500], 459 | [950, 600], [900, 650], 460 | [850, 600], [600, 700], 461 | [700, 800], 462 | [600, 900]]])) 463 | elif idx == 5: 464 | field_poly = FieldPoly(shg.Polygon([[0, 0], [400, 0], [400, 500], [200, 500], [200, 600], [500, 600], [500, 0], 465 | [1000, 0], [1000, 450], [700, 450], [700, 550], [1000, 550], [1000, 1000], 466 | [0, 1000]], holes=[[[200, 800], [300, 700], [600, 800], [300, 900]], 467 | [[700, 900], [700, 700], [800, 700], 468 | [800, 900]]])) 469 | elif idx == 6: 470 | field_poly = FieldPoly(shg.Polygon([[0, 0], [1000, 0], [1000, 1000], [0, 1000]])) 471 | else: 472 | field_poly = FieldPoly.synthesize(cities_count=5, hole_count=1, hole_cities_count=5, poly_extent=1000) 473 | 474 | if show: 475 | plt.figure(figsize=(20, 20)) 476 | initial_lines_count = len(get_field_lines(field_poly, show=False)) 477 | lines = get_field_lines(field_poly) 478 | 479 | for line in lines: 480 | line = shg.LineString(line) 481 | p1, p2 = line.coords.xy 482 | plt.plot(p1, p2, lw=5) 483 | 484 | field_poly.plot(lw=5) 485 | temp_x, temp_y = field_poly.polygon.centroid.xy 486 | temp_x, temp_y = float(temp_x[0]), float(temp_y[0]) 487 | plt.text(temp_x, temp_y, f"Cost: {initial_lines_count}", ha='center', va='center', fontsize=40) 488 | plt.gca().set_aspect('equal', 'box') 489 | plt.grid(axis='both') 490 | plt.title("Original Polygon", fontsize=50) 491 | plt.xticks(fontsize=30) 492 | plt.yticks(fontsize=30) 493 | plt.tight_layout() 494 | plt.show() 495 | 496 | return field_poly 497 | 498 | 499 | def swap_indices(distance_matrix, idx): 500 | new_distance_matrix = np.array(distance_matrix) 501 | new_distance_matrix[[0, idx]] = new_distance_matrix[[idx, 0]] 502 | new_distance_matrix[:, [0, idx]] = new_distance_matrix[:, [idx, 0]] 503 | new_distance_matrix[:, 0] = 0 504 | return new_distance_matrix 505 | 506 | 507 | def best_LKH_path(E, R, distance_matrix, mapping): 508 | lowest_cost = 1e10 509 | lowest_path = None 510 | 511 | for i in range(len(distance_matrix) - 1): 512 | for j in range(i + 1, len(distance_matrix)): 513 | path = solve_tsp(distance_matrix, endpoints=(i, j)) 514 | unique_edges = set() 515 | cost = 0 516 | 517 | for k in range(len(path) - 1): 518 | p1 = mapping[path[k]] 519 | p2 = mapping[path[k + 1]] 520 | 521 | if (p1, p2) in R and (p1, p2) not in unique_edges: 522 | unique_edges.add((p1, p2)) 523 | 524 | cost += distance_matrix[path[k], path[k + 1]] 525 | 526 | if cost < lowest_cost: 527 | lowest_cost = cost 528 | lowest_path = path 529 | 530 | # for i in range(len(distance_matrix)): 531 | # new_distance_matrix = swap_indices(distance_matrix, i) 532 | # path = solve_tsp_simulated_annealing(new_distance_matrix)[0] 533 | # # path = elkai.solve_float_matrix(new_distance_matrix) 534 | # unique_edges = set() 535 | # cost = 0 536 | # 537 | # for i in range(len(path) - 1): 538 | # p1 = mapping[path[i]] 539 | # p2 = mapping[path[i + 1]] 540 | # 541 | # if (p1, p2) in R and (p1, p2) not in unique_edges: 542 | # unique_edges.add((p1, p2)) 543 | # 544 | # cost += new_distance_matrix[path[i], path[i+1]] 545 | # 546 | # if cost > lowest_cost: 547 | # lowest_cost = cost 548 | # lowest_path = path 549 | 550 | unique_edges = set() 551 | covered_required = 0 552 | 553 | for edge in E: 554 | plt.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], c='skyblue') 555 | 556 | for i in range(len(lowest_path) - 1): 557 | p1 = mapping[lowest_path[i]] 558 | p2 = mapping[lowest_path[i + 1]] 559 | xs = np.linspace(p1[0], p2[0], 100) 560 | ys = np.linspace(p1[1], p2[1], 100) 561 | line = plt.plot(xs, ys, c='orange', linewidth=2)[0] 562 | add_arrow(line, color='green') 563 | 564 | if i == 0: 565 | plt.plot(p1[0], p1[1], marker='o', color='blue', markersize=5) 566 | if i == len(lowest_path) - 2: 567 | plt.plot(p2[0], p2[1], marker='o', color='red', markersize=5) 568 | 569 | if (p1, p2) in R and (p1, p2) not in unique_edges: 570 | unique_edges.add((p1, p2)) 571 | covered_required += 1 572 | 573 | logger.info(f"\n-------------LKH Solver Results-------------") 574 | logger.info(f"Covered required edges: {covered_required} out of {len(R) // 2}") 575 | logger.info(f"Path cost: {lowest_cost}") 576 | plt.gca().set_aspect('equal', 'box') 577 | plt.grid(axis='both') 578 | plt.title("LKH Solver") 579 | plt.tight_layout() 580 | plt.show() 581 | 582 | 583 | def best_a_star_path(V, E, R, distance_matrix, mapping): 584 | min_cost = config['DEFAULT_EDGE_COST'] 585 | max_covered = -1 586 | min_path = None 587 | 588 | plt.figure(figsize=(20, 20)) 589 | 590 | for i in range(len(V) - 1): 591 | for j in range(i + 1, len(V)): 592 | unique_edges = set() 593 | covered_required = 0 594 | path = find_a_star_path(distance_matrix, mapping, start=V[i], end=V[j]) 595 | 596 | # for k in range(len(path) - 1): 597 | # p1 = tuple(map(float, path[k][0][1:-1].split(','))) 598 | # p2 = tuple(map(float, path[k + 1][0][1:-1].split(','))) 599 | # 600 | # if (p1, p2) in R and (p1, p2) not in unique_edges: 601 | # unique_edges.add((p1, p2)) 602 | # covered_required += 1 603 | # 604 | # if covered_required >= max_covered: 605 | # max_covered = covered_required 606 | # min_path = path 607 | 608 | cost = float(path[-1][1]) 609 | 610 | if cost < min_cost: 611 | min_cost = cost 612 | min_path = path 613 | 614 | path = min_path 615 | 616 | for edge in E: 617 | plt.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], c='skyblue', lw=5) 618 | 619 | unique_edges = set() 620 | covered_required = 0 621 | 622 | for i in range(len(path) - 1): 623 | p1 = tuple(map(float, path[i][0][1:-1].split(','))) 624 | p2 = tuple(map(float, path[i + 1][0][1:-1].split(','))) 625 | xs = np.linspace(p1[0], p2[0], 100) 626 | ys = np.linspace(p1[1], p2[1], 100) 627 | line = plt.plot(xs, ys, c='orange', linewidth=5)[0] 628 | add_arrow(line, color='green') 629 | 630 | if i == 0: 631 | plt.plot(p1[0], p1[1], marker='o', color='blue', markersize=15) 632 | if i == len(path) - 2: 633 | plt.plot(p2[0], p2[1], marker='o', color='red', markersize=15) 634 | 635 | if (p1, p2) in R and (p1, p2) not in unique_edges: 636 | unique_edges.add((p1, p2)) 637 | covered_required += 1 638 | 639 | logger.info(f"\n-------------A* Search Results-------------") 640 | logger.info(f"Covered required edges: {covered_required} out of {len(R) // 2}") 641 | logger.info(f"Path cost: {min_cost}") 642 | plt.text(500, 500, f"Cost: {covered_required}", ha='center', va='center', fontsize=40) 643 | plt.gca().set_aspect('equal', 'box') 644 | plt.grid(axis='both') 645 | plt.title("A* Search", fontsize=50) 646 | plt.xticks(fontsize=30) 647 | plt.yticks(fontsize=30) 648 | plt.tight_layout() 649 | plt.show() 650 | 651 | 652 | def main(): 653 | field_poly = choose_region(idx=1, show=True) 654 | optimum_polygons = perform_optimization(field_poly, use_mp=False) 655 | plot_optimum_polygons(optimum_polygons, field_poly) 656 | V, E, R = polygons_to_graph(optimum_polygons, show=True) 657 | distance_matrix, mapping = get_distance_matrix(V, E, R) 658 | 659 | # Finding a path using LKH TSP Solver 660 | # best_LKH_path(E, R, distance_matrix, mapping) 661 | 662 | # Finding a path using A* Search 663 | best_a_star_path(V, E, R, distance_matrix, mapping) 664 | 665 | 666 | if __name__ == "__main__": 667 | main() 668 | --------------------------------------------------------------------------------