├── .gitignore ├── DARPShapefiles.ipynb ├── DARPWithCPPAlgos.ipynb ├── README.md ├── ShortestL1Fuelling.ipynb ├── cpp_algorithms ├── __init__.py ├── common_helpers.py ├── constants.py ├── conversion │ ├── __init__.py │ ├── conversion.py │ └── conversion_helpers.py ├── coverage_path │ ├── __init__.py │ ├── bcd │ │ ├── __init__.py │ │ ├── astar.py │ │ ├── bcd.py │ │ └── bcd_helper.py │ ├── pathing_helpers.py │ ├── stc │ │ ├── __init__.py │ │ ├── stc.py │ │ ├── stc_caller.py │ │ └── stc_helpers.py │ └── wavefront │ │ ├── __init__.py │ │ ├── wavefront.py │ │ ├── wavefront_caller.py │ │ └── wavefront_helpers.py ├── cpp.py ├── darp │ ├── __init__.py │ ├── continuity.py │ ├── darp.py │ ├── darp_helpers.py │ └── iterate.py ├── dist_fill.py ├── fuel_path │ ├── __init__.py │ ├── fuel_path.py │ └── fuel_path_helpers.py ├── run_coverage.py └── testers │ ├── __init__.py │ ├── display_funcs.py │ ├── metrics.py │ └── testers.py ├── demo_media ├── bous.png ├── darp.png ├── kmstcr.png ├── kmstcsh.png ├── sptc.png ├── stcfuel.gif ├── stctests.png └── wavf.png ├── test_maps ├── caves_0.png ├── caves_1.png ├── center_0.png ├── center_1.png ├── comb_0.png ├── comb_1.png ├── comb_11.png ├── comb_12.png ├── comb_2.png ├── comb_3.png ├── comb_4.png ├── comb_5.png ├── comb_6.png ├── comb_7.png ├── comb_8.png ├── comb_9.png ├── corners_0.png ├── pipes_0.png ├── pipes_1.png └── pipes_2.png ├── test_notebooks ├── BoustrophedonTests.ipynb ├── CPPTests.ipynb ├── DARPTests.ipynb └── WavefrontTests.ipynb └── test_shps ├── juhu.zip ├── kamathipura.zip ├── mumb.zip └── paris.zip /.gitignore: -------------------------------------------------------------------------------- 1 | # Test folder and settings 2 | sandbox/ 3 | .vscode/ 4 | BlindScan.ipynb 5 | 6 | # Byte-compiled / optimized / DLL files 7 | __pycache__/ 8 | *.py[cod] 9 | *$py.class 10 | 11 | # C extensions 12 | *.so 13 | 14 | # Distribution / packaging 15 | .Python 16 | build/ 17 | develop-eggs/ 18 | dist/ 19 | downloads/ 20 | eggs/ 21 | .eggs/ 22 | lib/ 23 | lib64/ 24 | parts/ 25 | sdist/ 26 | var/ 27 | wheels/ 28 | pip-wheel-metadata/ 29 | share/python-wheels/ 30 | *.egg-info/ 31 | .installed.cfg 32 | *.egg 33 | MANIFEST 34 | 35 | # PyInstaller 36 | # Usually these files are written by a python script from a template 37 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 38 | *.manifest 39 | *.spec 40 | 41 | # Installer logs 42 | pip-log.txt 43 | pip-delete-this-directory.txt 44 | 45 | # Unit test / coverage reports 46 | htmlcov/ 47 | .tox/ 48 | .nox/ 49 | .coverage 50 | .coverage.* 51 | .cache 52 | nosetests.xml 53 | coverage.xml 54 | *.cover 55 | *.py,cover 56 | .hypothesis/ 57 | .pytest_cache/ 58 | 59 | # Translations 60 | *.mo 61 | *.pot 62 | 63 | # Django stuff: 64 | *.log 65 | local_settings.py 66 | db.sqlite3 67 | db.sqlite3-journal 68 | 69 | # Flask stuff: 70 | instance/ 71 | .webassets-cache 72 | 73 | # Scrapy stuff: 74 | .scrapy 75 | 76 | # Sphinx documentation 77 | docs/_build/ 78 | 79 | # PyBuilder 80 | target/ 81 | 82 | # Jupyter Notebook 83 | .ipynb_checkpoints 84 | 85 | # IPython 86 | profile_default/ 87 | ipython_config.py 88 | 89 | # pyenv 90 | .python-version 91 | 92 | # pipenv 93 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 94 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 95 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 96 | # install all needed dependencies. 97 | #Pipfile.lock 98 | 99 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 100 | __pypackages__/ 101 | 102 | # Celery stuff 103 | celerybeat-schedule 104 | celerybeat.pid 105 | 106 | # SageMath parsed files 107 | *.sage.py 108 | 109 | # Environments 110 | .env 111 | .venv 112 | env/ 113 | venv/ 114 | ENV/ 115 | env.bak/ 116 | venv.bak/ 117 | 118 | # Spyder project settings 119 | .spyderproject 120 | .spyproject 121 | 122 | # Rope project settings 123 | .ropeproject 124 | 125 | # mkdocs documentation 126 | /site 127 | 128 | # mypy 129 | .mypy_cache/ 130 | .dmypy.json 131 | dmypy.json 132 | 133 | # Pyre type checker 134 | .pyre/ 135 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Coverage Path Planning 2 | Contains python implementations of various algorithms used for coverage path planning and one for area division used in mCPP. 3 | 4 | ## DARP - Area Division 5 | DARP : Divide Areas Algorithm for Optimal Multi-Robot Coverage Path, is an equal area division algorithm for multiple robot coverage paths. 6 | 7 | - DARP visualization on a test area map. 8 | - Left most images are area maps that have roughly the same number of assigned cells. 9 | - Point gradients are traversable L1 distance based. 10 | - Region gradients pertain to the computation of the Ci matrix as given in the paper. 11 | 12 | ![DARP](./demo_media/darp.png) 13 | [Image source notebook](./test_notebooks/DARPTests.ipynb). [Preprint](http://kapoutsis.info/wp-content/uploads/2017/02/j3.pdf) and [Repository](https://github.com/athakapo/DARP). 14 | 15 | --- 16 | ## Coverage Path Planning 17 | - Gradients are created from the starting points. 18 | - Green points are the start points, red are the end points and white is the coverage path. 19 | - [Images source notebook](./DARPWithCPPAlgos.ipynb). 20 | ### Spanning Tree Coverage 21 | Creates a minimum spanning tree from the given area for generation of the coverage path. 22 | STC is run individually on the sub regions. 23 | ![DARP and STC](./demo_media/sptc.png) 24 | 25 | ### Boustrophedon 26 | An online algorithm that uses A* to get to the next starting point. 27 | Boustrophedon is run individually on the sub regions. 28 | ![DARP and Bous](./demo_media/bous.png) 29 | 30 | ### Wavefront 31 | An algorithm that uses a distance to central point based gradient to traverse the region. 32 | Waverfront is run individually on the sub regions. 33 | ![DARP and Wavefront](./demo_media/wavf.png) 34 | 35 | --- 36 | 37 | ## Other Stuff 38 | 39 | ### Fuelling Algo 40 | Uses the shortest L1 path to get to the fuel point from a point in the coverage path that minimizes the residual fuel. The Pink 41 | - Pink path is the coverage path, light blue is the fuelling path. 42 | - Yellow is the start point and red is the end point. 43 | - The algorithm displayed is STC. 44 | - [Animation source notebook](./test_notebooks/CPPTests.ipynb), [algo notebook](./ShortestL1Fuelling.ipynb). 45 | ![Fuelling while STC](./demo_media/stcfuel.gif) 46 | 47 | ### Tests 48 | Tests on the CPP and Fuelling algos with metrics are [here](./test_notebooks/CPPTests.ipynb). 49 | Tests on STC running on the test area maps. 50 | ![STCTests](./demo_media/stctests.png) 51 | 52 | ### Running on Shapefiles 53 | The Area division and coverage path algorithms were run on shapefiles pertaining to actual areas but reprojection of the generated path is (highly) inaccurate. 54 | 55 | The rasteriszed map with the path overlay. 56 | ![Raster](./demo_media/kmstcr.png) 57 | 58 | The shapefile on a map tile with the reprojected path as an overlay. 59 | ![ShapefileOverlay](./demo_media/kmstcsh.png) 60 | More [here](./DARPShapefiles.ipynb) (execrable reprojections, pls hlp). 61 | 62 | --- 63 | 64 | **Should Do** 65 | - Fix issue with DARP. 66 | - Add latest optimizaiton of STC. 67 | 68 | 69 | -------------------------------------------------------------------------------- /cpp_algorithms/__init__.py: -------------------------------------------------------------------------------- 1 | # Main caller 2 | from .cpp import cpp 3 | 4 | # Helper functions 5 | from .dist_fill import dist_fill 6 | from .common_helpers import is_valid, adjacency_test 7 | from .common_helpers import plot, imshow, imshow_scatter 8 | from .common_helpers import get_all_area_maps, get_random_coords, get_drone_map 9 | 10 | # Division function 11 | from .darp import darp 12 | 13 | # Main path functions 14 | from .coverage_path import bcd 15 | from .coverage_path import stc 16 | from .coverage_path import wavefront 17 | 18 | # Fuel path functions 19 | from .fuel_path import splice_paths 20 | from .fuel_path import get_fuel_paths 21 | 22 | # Testing functions 23 | from .testers import single_robot_single 24 | from .testers import single_robot_multiple 25 | 26 | # Rasterization functions 27 | from .conversion import conversion 28 | from .run_coverage import run_coverage -------------------------------------------------------------------------------- /cpp_algorithms/common_helpers.py: -------------------------------------------------------------------------------- 1 | """ 2 | Some helper functions that are often used. 3 | """ 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | from .constants import OB, NO 7 | from pathlib import Path 8 | from PIL import Image 9 | 10 | RES = [(32, 32),(50, 50),(50, 144),(144, 255),(256,256)] 11 | 12 | def adjacency_test(path, exactly_one=True): 13 | """ 14 | Checks all points in a path for L1 adjacency. 15 | """ 16 | prev_point = path[0] 17 | for i,point in enumerate(path[1:]): 18 | x,y = prev_point 19 | x_,y_ = point 20 | 21 | dist = np.abs(x - x_) + np.abs(y - y_) 22 | prev_point = point 23 | if exactly_one and dist == 1: 24 | continue 25 | elif dist <= 1: 26 | continue 27 | else: 28 | return i - 1 29 | return True 30 | 31 | def generate_no_obs_area_map(resolutions=RES): 32 | """ 33 | resolutions : list of tuples [(rows, cols)] 34 | """ 35 | area_maps = [] 36 | for res in resolutions: 37 | area_maps.append(np.zeros(res)) 38 | return area_maps 39 | 40 | def generate_point_obstacles(area_map, p=0.5): 41 | """ 42 | Adds point obstacles to the area_map with the given 43 | probability `p`, if `p==1` then the entire map will 44 | be covered. 45 | """ 46 | area_map = area_map.copy() 47 | area_map[np.random.rand(*area_map.shape)= g or y >= h 130 | if lesser or greater: 131 | return False 132 | return True 133 | 134 | def is_valid(coord, area_map, obstacle = -1): 135 | """ 136 | Check is a coord (x,y) is bounded and not 137 | on an obstacle. 138 | """ 139 | coord = tuple(coord) 140 | is_b = is_bounded(coord, area_map.shape) 141 | 142 | if is_b: 143 | is_on_obs = False 144 | if isinstance(obstacle, list): 145 | for obs in obstacle: 146 | is_on_obs |= area_map[coord] == obs 147 | else: 148 | is_on_obs = area_map[coord] == obstacle 149 | if not is_on_obs: 150 | return True 151 | return False 152 | 153 | def get_all_area_maps(folder_path): 154 | """ 155 | Returns size sorted list of area maps. 156 | 157 | folder_path : path to the folder contiaining the maps (.png) 158 | """ 159 | ams = [] 160 | for path in Path(folder_path).iterdir(): 161 | try: 162 | ams.append(get_area_map(path)) 163 | except: 164 | continue 165 | 166 | am_idx = np.array([am.size for am in ams]).argsort() 167 | return list(np.array(ams)[am_idx]) 168 | 169 | def get_drone_map(A, i, obstacle=OB, coverage=NO): 170 | """ 171 | Returns area map for a single drone 172 | from the assignment matrix. 173 | 174 | PARAMETERS 175 | --- 176 | A : assignment matrix obtained from darp. 177 | i : the drone number (cell value of A). 178 | obstacle : value to assign the obstacle 179 | coverage : value to assign the coverage area 180 | """ 181 | am = A.copy() 182 | x,y = np.where(am != i) 183 | am[x,y] = obstacle 184 | x,y = np.where(am == i) 185 | am[x,y] = coverage 186 | return am -------------------------------------------------------------------------------- /cpp_algorithms/constants.py: -------------------------------------------------------------------------------- 1 | OB = -1 # Obstacle value, don't change! 2 | FU = 0 # Value of fuel on the fuel dist_map 3 | NO = 0 # Area to be mapped value 4 | 5 | # Conversion constants 6 | START = "drone" 7 | FUEL = "fuel" 8 | 9 | # GeoJSON Keys 10 | KEY = "type" 11 | COVERAGE = "coverage" 12 | OBSTACLE = "obstacle" 13 | AREAS = [COVERAGE, OBSTACLE] 14 | POINTS = [START, FUEL] 15 | FEATURES = [*AREAS, *POINTS] 16 | POINT = "layers/POINT" 17 | POLYG = "layers/POLYGON" 18 | 19 | # Proojection constants 20 | EPSG = 4326 21 | CRS = f"EPSG:{EPSG}" 22 | 23 | # Geometry 24 | GEOM_POINT = "Point" 25 | GEOM_POLYG = "Polygon" -------------------------------------------------------------------------------- /cpp_algorithms/conversion/__init__.py: -------------------------------------------------------------------------------- 1 | from .conversion import conversion -------------------------------------------------------------------------------- /cpp_algorithms/conversion/conversion.py: -------------------------------------------------------------------------------- 1 | from .conversion_helpers import get_scale, get_gpdframe 2 | from .conversion_helpers import create_gdframe 3 | from .conversion_helpers import get_features_dict, get_final_coverage_polygon 4 | import json 5 | import shapely 6 | import geopandas as gpd 7 | import numpy as np 8 | from geopy import distance 9 | from skimage import measure 10 | from skimage.draw import polygon 11 | from cpp_algorithms.constants import CRS, EPSG, KEY, GEOM_POINT 12 | 13 | 14 | def get_raster(gpdf_final, scale=2000, CRS=CRS): 15 | assert len(gpdf_final) == 1 16 | try: 17 | shp = gpdf_final.to_crs(crs=CRS) 18 | except: 19 | shp = gpdf_final.set_crs(crs=CRS) 20 | 21 | ext = np.array(shp.geometry[0].exterior).copy() 22 | ite = map(np.array, shp.geometry[0].interiors) 23 | 24 | mn = ext.min(axis=0) 25 | mix = ext - mn 26 | mx = mix.max() 27 | mix *= scale/mx 28 | mix = np.int64(mix) 29 | sh = mix.max(axis=0) 30 | 31 | r, c = polygon(*mix.T, sh) 32 | p = np.full(mix.max(axis=0), -1) 33 | p[r, c] = 0 34 | 35 | for o in ite: 36 | r, c = polygon(*np.int16((o-mn)*scale/mx).T, sh) 37 | p[r, c] = -1 38 | 39 | return p, mn, mx 40 | 41 | 42 | def coo_to_points(gpdf_points, mn, mx, key=KEY, scale=2000): 43 | types = [] 44 | points = [] 45 | for p in gpdf_points.iterrows(): 46 | if p[1][key] not in types: 47 | types.append(p[1][key]) 48 | points.append([]) 49 | 50 | i = types.index(p[1][key]) 51 | coords = np.array(p[1].geometry.coords) 52 | points[i].append(tuple(np.int64((coords - mn)*scale/mx)[0])) 53 | return points, types 54 | 55 | 56 | def down_sample(side, area_map, points, meter=1): 57 | st = int(side/meter) 58 | area_map = area_map.copy() 59 | area_map[area_map == -1] = 1 60 | vals = [] 61 | for i, point in enumerate(points): 62 | point = np.array(point) 63 | x, y = point.T 64 | area_map[x, y] = i+2 65 | vals.append(i+2) 66 | 67 | temp = measure.block_reduce(area_map, (st, st), np.max, cval=0) 68 | temp[temp == 1] = -1 69 | points = [] 70 | for val in vals: 71 | points_ = np.stack(np.where(temp == val)).T 72 | points.append(list(map(tuple, points_))) 73 | temp[temp == val] = 0 74 | return temp, points 75 | 76 | 77 | def conversion(side, geojson): 78 | """ 79 | side : drone area of coverage square's side in meters. 80 | geo_json : parsed json in the geojson fromat from the frontend. 81 | """ 82 | if isinstance(geojson, str): 83 | geojson = json.loads(geojson) 84 | 85 | # contains the GeopandasDataFrames for all featureCollections 86 | gpdf_all = get_gpdframe(geojson) 87 | features = get_features_dict(gpdf_all) 88 | 89 | # Unnecessary repitition someone improve it 90 | gpdf_final = create_gdframe( 91 | features, no_points=True).set_crs(epsg=EPSG) 92 | final_coverage_polygon = gpdf_final.geometry[0] 93 | 94 | if gpdf_all[0].geom_type[0] == GEOM_POINT: 95 | gpdf_points = gpdf_all[0] 96 | else: 97 | gpdf_points = gpdf_all[1] 98 | gpdf_points = gpdf_points.set_crs(epsg=EPSG) 99 | 100 | # If the scale is large it takes a lot of time to compute 101 | m = 1 102 | scale = get_scale(final_coverage_polygon, meter=m) 103 | while scale > 3000: 104 | m *= 2 105 | scale = get_scale(final_coverage_polygon, meter=m) 106 | 107 | area_map_r, mn, mx = get_raster(gpdf_final, scale, CRS=CRS) 108 | points_r, types = coo_to_points(gpdf_points, mn, mx, scale=scale) 109 | area_map, points = down_sample(side, area_map_r, points_r, meter=m) 110 | 111 | # print(mn, mx, scale, area_map.shape, m, area_map_r.shape) 112 | def retransformer(cp): return ((np.array(cp)*side) * 113 | mx/(scale * m))+mn[None, None, :] + 1e-4 114 | 115 | return area_map, points, types, retransformer 116 | -------------------------------------------------------------------------------- /cpp_algorithms/conversion/conversion_helpers.py: -------------------------------------------------------------------------------- 1 | import json 2 | import numpy as np 3 | import shapely.geometry 4 | import geopandas as gpd 5 | from geopy import distance 6 | from cpp_algorithms.constants import KEY, COVERAGE, OBSTACLE, POINT 7 | from cpp_algorithms.constants import AREAS, POINTS, FEATURES, POLYG 8 | from cpp_algorithms.constants import CRS, GEOM_POINT, GEOM_POLYG 9 | 10 | 11 | def get_features_dict(shapefiles, key=KEY, fnames=FEATURES): 12 | """ 13 | Name of the features should be shapefile 14 | `key` column values 15 | """ 16 | features = {} 17 | for name in fnames: 18 | features[name] = [] 19 | 20 | for sh in shapefiles: 21 | for rows in sh.iterrows(): 22 | for k in features: 23 | if rows[1][key].find(k) >= 0: 24 | features[k].append(rows[1].geometry) 25 | return features 26 | 27 | 28 | def get_final_coverage_polygon(features): 29 | """ 30 | Union of coverage and difference of 31 | obstacles. 32 | """ 33 | final = features[COVERAGE][0] 34 | for cov in features[COVERAGE]: 35 | final = final.union(cov) 36 | for obs in features[OBSTACLE]: 37 | final = final.difference(obs) 38 | return final 39 | 40 | 41 | def get_gpdframe(geojson): 42 | if isinstance(geojson, list): 43 | # Takes care of : [FeatureCollection,...,FeaturesCollection] 44 | return list(map(gpd.GeoDataFrame.from_features, geojson)) 45 | else: 46 | # Geojson type not property type. 47 | if geojson['type'] == "FeatureCollection": 48 | all_ = gpd.GeoDataFrame.from_features(geojson['features']) 49 | return [ 50 | all_[all_.geom_type == GEOM_POINT], 51 | all_[all_.geom_type == GEOM_POLYG] 52 | ] 53 | else: 54 | # Might break the code ahead 55 | return gpd.GeoDataFrame.from_features(geojson) 56 | 57 | 58 | # Terrible naming conventions I know. 59 | def create_gdframe(features, crs=CRS, no_points=False): 60 | """ 61 | Create GeoDataFrame from features 62 | """ 63 | final_coverage = get_final_coverage_polygon(features) 64 | points = [] 65 | if not no_points: 66 | for d in features[START]: 67 | points.append({ 68 | KEY: START, 69 | 'geometry': d 70 | }) 71 | for f in features[FUEL]: 72 | points.append({ 73 | KEY: FUEL, 74 | 'geometry': d 75 | }) 76 | points.append({ 77 | KEY: COVERAGE, 78 | 'geometry': final_coverage}) 79 | return gpd.GeoDataFrame(points, crs=crs) 80 | 81 | 82 | def get_hv_wh(final_coverage_polygon): 83 | """ 84 | Get haversine calcualted width and height of 85 | the smallest bounding rectangle of the coverage area. 86 | """ 87 | llng, llat, rlng, rlat = final_coverage_polygon.bounds 88 | ll = (llat, llng) 89 | lr = (llat, rlng) 90 | tr = (rlat, rlng) 91 | tl = (rlat, llng) 92 | w = distance.distance(ll, lr) 93 | h = distance.distance(ll, tl) 94 | return w, h 95 | 96 | 97 | def get_scale(final_coverage_polygon, meter=1): 98 | """ 99 | meter : 1 pixel == ? meters 100 | """ 101 | w, h = get_hv_wh(final_coverage_polygon) 102 | w = w.m 103 | h = h.m 104 | return int(np.round((np.array([w, h])/meter).max())) 105 | -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/__init__.py: -------------------------------------------------------------------------------- 1 | from .bcd import bcd 2 | from .stc import stc 3 | from .wavefront import wavefront -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/bcd/__init__.py: -------------------------------------------------------------------------------- 1 | from .bcd import bcd -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/bcd/astar.py: -------------------------------------------------------------------------------- 1 | def heuristic(start, goal): 2 | # Use Chebyshev distance heuristic if we can move one square either 3 | #adjacent or diagonal 4 | D = 1 5 | D2 = 1 6 | dx = abs(start[0] - goal[0]) 7 | dy = abs(start[1] - goal[1]) 8 | return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy) 9 | 10 | 11 | def get_vertex_neighbours(pos, diameter, width, height): 12 | n = [] 13 | # Moves allow link a chess king 14 | for dx, dy in [(diameter, 0), (-diameter, 0), (0, diameter), (0, -diameter)]: 15 | x2 = pos[0] + dx 16 | y2 = pos[1] + dy 17 | if x2 < 0 or x2 > width-1 or y2 < 0 or y2 > height-1: 18 | continue 19 | n.append((x2, y2)) 20 | return n 21 | 22 | 23 | def astar_search(start, end, graph, diameter, width, height): 24 | 25 | G = {} # Actual movement cost to each position from the start position 26 | F = {} # Estimated movement cost of start to end going via this position 27 | 28 | # Initialize starting values 29 | G[start] = 0 30 | F[start] = heuristic(start, end) 31 | 32 | closedVertices = set() 33 | openVertices = set([start]) 34 | cameFrom = {} 35 | 36 | # Adding a stop condition 37 | outer_iterations = 0 38 | max_iterations = (len(graph[0]) * len(graph) // 2) 39 | 40 | while len(openVertices) > 0: 41 | outer_iterations += 1 42 | 43 | # Get the vertex in the open list with the lowest F score 44 | current = None 45 | currentFscore = None 46 | for pos in openVertices: 47 | if current is None or F[pos] < currentFscore: 48 | currentFscore = F[pos] 49 | current = pos 50 | 51 | # Check if we have reached the goal 52 | if current == end: 53 | # Retrace our route backward 54 | path = [current] 55 | while current in cameFrom: 56 | current = cameFrom[current] 57 | path.append(current) 58 | path.reverse() 59 | return path # Done! 60 | 61 | # Mark the current vertex as closed 62 | openVertices.remove(current) 63 | closedVertices.add(current) 64 | 65 | # Update scores for vertices near the current position 66 | for neighbour in get_vertex_neighbours(current, diameter, width, height): 67 | if neighbour in closedVertices: 68 | continue # We have already processed this node exhaustively 69 | x = neighbour[0] 70 | y = neighbour[1] 71 | if graph[x][y] != 150: 72 | continue 73 | else: 74 | candidateG = G[current] + 1 75 | 76 | if neighbour not in openVertices: 77 | openVertices.add(neighbour) # Discovered a new vertex 78 | 79 | elif candidateG >= G[neighbour]: 80 | continue # This G score is worse than previously found 81 | 82 | # Adopt this G score 83 | cameFrom[neighbour] = current 84 | G[neighbour] = candidateG 85 | H = heuristic(neighbour, end) 86 | F[neighbour] = G[neighbour] + H 87 | 88 | raise RuntimeError("A* failed to find a solution") 89 | 90 | def astar_path(M, Memory, path): 91 | for q in path: 92 | x=q[0] 93 | y=q[1] 94 | Memory.append((x, y)) -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/bcd/bcd.py: -------------------------------------------------------------------------------- 1 | from .bcd_helper import backtracking_list, move_like_oxen, bcd_preprocess 2 | from .astar import astar_path, astar_search 3 | 4 | def bcd_main(matrix, start_point): 5 | width, height = matrix.shape 6 | # May add support for variable radius. 7 | radius = 0.5 8 | diameter = int(2*radius) 9 | x, y = start_point 10 | memory = [] 11 | backtrack_counts = 0 12 | point_find_failed = 0 13 | while True: 14 | critical_x, critical_y = move_like_oxen(matrix, diameter, x, y, memory) 15 | next_, is_end = backtracking_list( 16 | memory, diameter, matrix, critical_x, critical_y) 17 | x, y = next_ 18 | if is_end: 19 | break 20 | else: 21 | start = (critical_x, critical_y) 22 | end = (x, y) 23 | path = astar_search(start, end, matrix, diameter, width, height) 24 | astar_path(matrix, memory, path) 25 | return memory 26 | 27 | def bcd(area_map, start_point): 28 | """ 29 | Runs the boustrophedon cell decomposition algorithm 30 | with astar for point search and returns th generated path. 31 | 32 | PARAMETERS 33 | --- 34 | area_map : Area map to be covered, 2-dim numpy array. 35 | coverage region = 0 36 | obstacle region = -1 37 | 38 | start_point : Drone deployment point on the `area_map`, tuple (x,y) 39 | 40 | RETURNS 41 | --- 42 | coverage_path : main coverage path. 43 | """ 44 | matrix = bcd_preprocess(area_map) 45 | coverage_path = bcd_main(matrix, start_point) 46 | return coverage_path 47 | -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/bcd/bcd_helper.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms.common_helpers import is_valid 3 | 4 | def bcd_preprocess(area_map): 5 | """ 6 | Returns matrix that in the form 7 | that is read by the main algorithm. 8 | """ 9 | matrix = np.full(area_map.shape, 0, dtype=np.uint8) 10 | matrix[area_map == 0] = 255 11 | return matrix 12 | 13 | def is_valid_vectorized(coords, matrix): 14 | assert coords.shape[1] == 2 15 | h,w = matrix.shape 16 | x,y = coords.T 17 | is_within_bounds = (x >= 0) & (x < h) & (y >= 0) & (y < w) 18 | x = np.clip(x.copy(), 0, h-1) 19 | y = np.clip(y.copy(), 0, w-1) 20 | is_not_on_obstacle = (matrix[x,y] != 0) & (matrix[x,y] != 150) 21 | return is_within_bounds & is_not_on_obstacle 22 | 23 | def backtracking_list(memory, _, matrix, x, y): 24 | bt_cond_points = { 25 | "r": lambda p: p + np.array([[0,1]]), # right 26 | "tr": lambda p: p + np.array([[-1,1]]), # top-right 27 | "t": lambda p: p + np.array([[-1,0]]), # top 28 | "tl": lambda p: p + np.array([[-1,-1]]), # top-left 29 | "l": lambda p: p + np.array([[0,-1]]), # left 30 | "bl": lambda p: p + np.array([[1,-1]]), # bottom-left 31 | "b": lambda p: p + np.array([[1,0]]), # bottom 32 | "br": lambda p: p + np.array([[1,1]]), # bottom-right 33 | } 34 | memory_ = np.array(memory) 35 | assert memory_.shape[1] == 2, "you've messed up something" 36 | 37 | eight_di = {k: bt_cond_points[k](memory_) for k in bt_cond_points} 38 | is_valid_eight = {k: is_valid_vectorized(eight_di[k], matrix) for k in eight_di} 39 | cond_a = np.int0(is_valid_eight["r"] & ~is_valid_eight["br"]) 40 | cond_b = np.int0(is_valid_eight["r"] & ~is_valid_eight["tr"]) 41 | cond_c = np.int0(is_valid_eight["l"] & ~is_valid_eight["bl"]) 42 | cond_d = np.int0(is_valid_eight["l"] & ~is_valid_eight["tl"]) 43 | cond_e = np.int0(is_valid_eight["b"] & ~is_valid_eight["bl"]) 44 | cond_f = np.int0(is_valid_eight["b"] & ~is_valid_eight["br"]) 45 | μ_of_s = (cond_a + cond_b+ cond_c + cond_d + cond_e + cond_f) 46 | 47 | backtrack_points = memory_[μ_of_s > 0] 48 | if backtrack_points.shape[0] == 0: 49 | backtrack_points = memory_[is_valid_eight["r"] | is_valid_eight["l"] | 50 | is_valid_eight["t"] | is_valid_eight["b"]] 51 | 52 | if backtrack_points.shape[0] == 0: 53 | return (0,0), True 54 | else: 55 | closest_point_idx = ((backtrack_points - np.array([x,y]))**2).sum(axis = 1).argmin() 56 | return tuple(backtrack_points[closest_point_idx]), False 57 | 58 | 59 | def visit(matrix, x, y, memory): 60 | matrix[(x,y)] = 150 # 150 == visited 61 | memory.append((x, y)) 62 | return x,y 63 | 64 | def move_like_oxen(matrix, diameter, x, y, memory): 65 | # TODO :: Variable diameter support 66 | udlr = { 67 | "u": lambda x,y : (x-diameter,y), 68 | "d": lambda x,y : (x+diameter,y), 69 | "l": lambda x,y : (x,y-diameter), 70 | "r": lambda x,y : (x,y+diameter) 71 | } 72 | u = "u";d = "d";r = "r";l = "l" 73 | visit(matrix, x, y, memory) 74 | 75 | while True: 76 | dir_ = [u,d,r,l] 77 | while len(dir_) > 0: 78 | d_ = dir_.pop(0) 79 | x_, y_ = udlr[d_](x,y) 80 | if is_valid((x_,y_), matrix, [0, 150]): 81 | x, y = visit(matrix, x_, y_, memory) 82 | break 83 | elif d_ == l: 84 | return x, y -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/pathing_helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms.common_helpers import is_valid, get_random_coords 3 | 4 | get_adj = lambda x,y :[ 5 | (x+1,y),(x-1,y),(x,y+1),(x,y-1) 6 | ] 7 | 8 | get_adj_8 = lambda x,y :[ 9 | (x+1,y),(x-1,y),(x,y+1),(x,y-1), 10 | (x+1,y+1),(x-1,y-1),(x-1,y+1),(x+1,y-1) 11 | ] 12 | 13 | def splice_paths(coverage_path, splice_indices, splice_segments): 14 | """ 15 | Splices in `splice_segments` at the given `splice_indices` for 16 | the given `coverage_path` 17 | """ 18 | assert len(splice_indices) == len(splice_segments), "length discrepancy" 19 | full_path_segments = [] 20 | last_idx = 0 21 | for i,idx in enumerate(splice_indices): 22 | seg = np.array(coverage_path[last_idx:idx]) 23 | if len(seg) > 0: 24 | full_path_segments.append(seg) 25 | seg = np.array(splice_segments[i]) 26 | if len(seg) > 0: 27 | full_path_segments.append(seg) 28 | last_idx = idx + 1 29 | 30 | full_path_segments.append(np.array(coverage_path[last_idx:])) 31 | return np.concatenate(full_path_segments) 32 | 33 | def has_isolated_areas(area_map, obstacle=-1): 34 | """ 35 | Flood fills the area to check if there are 36 | isolated areas. 37 | """ 38 | 39 | v_map = (area_map == obstacle).copy() 40 | f_point = get_random_coords(area_map, 1, obstacle)[0] 41 | to_visit = [f_point] 42 | 43 | while len(to_visit) > 0: 44 | l = len(to_visit) 45 | for point in to_visit: 46 | v_map[point] = True 47 | 48 | for i in range(l): 49 | for adj_point in get_adj(*to_visit.pop(0)): 50 | if is_valid(adj_point, area_map, obstacle) \ 51 | and not v_map[adj_point] \ 52 | and adj_point not in to_visit: 53 | to_visit.append(adj_point) 54 | 55 | if v_map.sum() == v_map.size: 56 | return False 57 | return True 58 | 59 | def get_step(path_map, next_point, obstacle=-1): 60 | min_d_val = path_map[next_point] 61 | possible_point = None 62 | for adj_point in get_adj(*next_point): 63 | if is_valid(adj_point, path_map, obstacle): 64 | d_val = path_map[adj_point] 65 | if d_val < min_d_val: 66 | min_d_val = d_val 67 | possible_point = adj_point 68 | return possible_point, min_d_val 69 | 70 | def get_path(path_map, start_point, end_point, obstacle=-1): 71 | """ 72 | Get the shortest (directed) l1 distance 73 | between `start_point`, `end_point` for a given `path_map`. 74 | 75 | (directed ∵ distance should be min at end_point) 76 | """ 77 | path = [start_point] 78 | end_val = path_map[end_point] 79 | cell = path_map[start_point] 80 | next_point = start_point 81 | while cell != end_val: 82 | next_point, cell = get_step(path_map, next_point, obstacle) 83 | path.append(next_point) 84 | return path 85 | 86 | def wave_find_map(start_point, end_point, area_map, obstacle=-1): 87 | """ 88 | Creates a path map, which is a dist map that terminates once 89 | the required point is found. 90 | """ 91 | AREA_VAL = 0 92 | d_map = np.int64(area_map.copy()) 93 | v_map = (area_map == obstacle).copy() 94 | to_visit = [end_point] 95 | def loop(): 96 | d_val = 1 97 | while len(to_visit) > 0: 98 | l = len(to_visit) 99 | for point in to_visit: 100 | d_map[point] = d_val 101 | v_map[point] = True 102 | if point == start_point: 103 | return 104 | # check if point is the start or something 105 | 106 | d_val += 1 107 | 108 | for i in range(l): 109 | for adj_point in get_adj(*to_visit.pop(0)): 110 | if is_valid(adj_point, area_map, obstacle) \ 111 | and not v_map[adj_point] \ 112 | and adj_point not in to_visit: 113 | to_visit.append(adj_point) 114 | 115 | loop() 116 | d_map[d_map == AREA_VAL] = obstacle 117 | return d_map 118 | 119 | def get_shortest_l1_path(start_point, end_point, area_map, obstacle=-1): 120 | """ 121 | Returns the shortest Manhattan path between the two points 122 | taking obstacles into consideration. 123 | """ 124 | path_map = wave_find_map(start_point, end_point, area_map, obstacle) 125 | return get_path(path_map, start_point, end_point, obstacle) -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/stc/__init__.py: -------------------------------------------------------------------------------- 1 | from .stc import stc -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/stc/stc.py: -------------------------------------------------------------------------------- 1 | from .stc_helpers import stc_preprocess 2 | from .stc_caller import stc_caller 3 | 4 | def stc(area_map, start_point): 5 | """ 6 | Runs spanning tree coverage to obtain a path 7 | for the given area_map. 8 | """ 9 | matrix = stc_preprocess(area_map) 10 | coverage_path = stc_caller(matrix) 11 | idx = coverage_path.index(start_point) 12 | coverage_path = [*coverage_path[idx:],*coverage_path[:idx]] 13 | return coverage_path 14 | -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/stc/stc_caller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def stc_caller(matrix): 4 | bound = matrix.shape 5 | 6 | #Step 2 7 | r = int(bound[0]/2) 8 | c = int(bound[1]/2) 9 | grid = [[((max(abs((r-1)/2-i), abs((c-1)/2-j))+1) if(all(pt for pg in matrix[2*i:2*(i+1),2*j:2*(j+1)] for pt in pg)) else -1) for j in range(c)] for i in range(r)] 10 | vertices = {} 11 | obstacles = {} 12 | for i in range(r): 13 | for j in range(c): 14 | if grid[i][j]>=0: 15 | vertices[(i, j)] = None 16 | elif not all(not matrix[2*i+ri][2*j+cj] for ri in range(2) for cj in range(2)): 17 | obstacles[(i, j)] = [] 18 | for ri in range(2): 19 | for jc in [ri, abs(ri-1)]: 20 | if matrix[2*i+ri][2*j+jc]: 21 | obstacles[(i, j)].append(1) 22 | matrix[2*i+ri][2*j+jc] = 191 23 | else: 24 | obstacles[(i, j)].append(0) 25 | 26 | s = list(vertices.keys())[0] 27 | # Step 3 28 | pathf = [] 29 | pathi = {} 30 | treef = {} 31 | tree = {} 32 | while len(vertices): 33 | pathi[s] = None 34 | del vertices[s] 35 | if s not in tree: tree[s] = np.full(4, 0) 36 | grid[s[0]][s[1]] = 0 37 | tmp = [0, 0, 0, 0] 38 | tmp[0] = grid[s[0]-1][s[1]] if s[0]-1>=0 and grid[s[0]-1][s[1]]!=-1 else -1 39 | tmp[1] = grid[s[0]][s[1]+1] if s[1]+1=0 and grid[s[0]][s[1]-1]!=-1 else -1 42 | grt = max(tmp) 43 | if grt>0: 44 | ind = tmp.index(grt) 45 | s = (s[0]-1, s[1]) if ind==0 else (s[0], s[1]+1) if ind==1 else (s[0]+1, s[1]) if ind==2 else (s[0], s[1]-1) 46 | else: 47 | pathf.append(pathi) 48 | pathi = {} 49 | treef.update(tree) 50 | tree = {} 51 | if len(vertices.keys()): 52 | s = list(vertices.keys())[0] 53 | 54 | for p in range(len(pathf)): 55 | path = list(pathf[p].keys()) 56 | for i in range(1, len(path)): 57 | ind = 0 if path[i-1][0]-1==path[i][0] else 1 if path[i-1][1]+1==path[i][1] else 2 if path[i-1][0]+1==path[i][0] else 3 58 | treef[path[i-1]][ind] = 1 59 | treef[path[i]][(ind+2)%4] = 1 60 | 61 | # Step 4 62 | a = np.array([len(i) for i in pathf]) 63 | b = np.argsort(a) 64 | allPaths = [list(pathf[i].keys()) for i in range(len(pathf))] 65 | edgePath = np.array([i for i in range(len(pathf))]) 66 | ejPath = [] 67 | 68 | def whichPath(ver): 69 | for i in range(len(pathf)): 70 | if ver in pathf[i]: return i 71 | return 0 72 | 73 | def mergePath(ind1, v1, v2): 74 | ind2 = edgePath[whichPath(v2)] 75 | if len(pathf[ind1])>len(pathf[ind2]): 76 | pathf[ind1].update(pathf[ind2]) 77 | edgePath[edgePath==ind2] = ind1 78 | else: 79 | pathf[ind2].update(pathf[ind1]) 80 | edgePath[edgePath==ind1] = ind2 81 | ejPath.append([v1, v2]) 82 | 83 | for index in range(len(b)-1): 84 | i = b[index] 85 | for j in allPaths[i]: 86 | x, y = j 87 | if x-1>=0 and grid[x-1][y]!=-1 and (x-1, y) not in pathf[edgePath[i]]: 88 | mergePath(edgePath[i], j, (x-1, y)) 89 | if y-1>=0 and grid[x][y-1]!=-1 and (x, y-1) not in pathf[edgePath[i]]: 90 | mergePath(edgePath[i], j, (x, y-1)) 91 | if x+1=0 else None 147 | elif i==1: 148 | return (ap[0]-1, ap[1]) if ap[0]-1>=0 else None 149 | elif i==2: 150 | return (ap[0], ap[1]+1) if ap[1]+1=0: 243 | a_o = nextPoint(a_, (i+1)%4) 244 | pa_o = ifInPath(a_o) 245 | if a_o!=None and pa_o==pao and insert2(ao, a_o, pao, a, a_): 246 | flaga = 2 247 | else: 248 | flaga = 1 249 | else: 250 | flaga = 1 251 | 252 | if obstacles[ogrid][i-1]==1: 253 | b = actualPoint(ogrid, i-1) 254 | b_ = nextPoint(b, i) 255 | pb_ = ifInPath(b_) 256 | if b_!=None and pb_==-1: 257 | bo = nextPoint(b, i-1) 258 | pbo = ifInPath(bo) 259 | if bo!=None and pbo>=0: 260 | b_o = nextPoint(b_, i-1) 261 | pb_o = ifInPath(b_o) 262 | if b_o!=None and pb_o==pbo and insert2(b_o, bo, pbo, b_, b): 263 | flagb = 2 264 | if flaga==1 and not insert2(b_, b, pbo, a_, a): 265 | flaga=0 266 | elif flaga==2: 267 | if pbo!=pao: 268 | finalPathf[pao][a][ao] = b 269 | finalPathf[pbo][b][a] = bo 270 | finalPathf[pbo][b][b_o] = a_ 271 | finalPathf[pao][a_][b_] = ao 272 | if len(finalPathf[pbo])>len(finalPathf[pao]): 273 | tmpo = pbo 274 | pbo = pao 275 | pao = tmpo 276 | finalPathf[pao].update(finalPathf[pbo]) 277 | 278 | del finalPathf[pbo] 279 | else: 280 | flagb = 1 281 | else: 282 | flagb = 1 283 | if flagb==1: 284 | if flaga==1: 285 | tmpcp = {} 286 | tmpcp[a] = {a_:b} 287 | tmpcp[b] = {a:b_} 288 | tmpcp[b_] = {b:a_} 289 | tmpcp[a_] = {b_:a} 290 | pao = addInFinalPath(tmpcp) 291 | elif flaga==2 and not insert2(a, a_, pao, b, b_): 292 | flagb=0 293 | if flaga==2 or (flaga==1 and flagb!=0): 294 | obstacles[ogrid][i] = -1 295 | obstacles[nextPoint(ogrid, i)][(i+1)%4] = -1 296 | if flagb==2 or (flagb==1 and flaga!=0): 297 | obstacles[ogrid][i-1] = -1 298 | obstacles[nextPoint(ogrid, i)][(i+2)%4] = -1 299 | if flaga==2 and flagb==0: 300 | #ind a backtr 301 | pao = joinPath(a, pao, (i+2)%4) 302 | joinPath(a_, pao, i) 303 | elif flagb==2 and flaga==0: 304 | #ind b backtr 305 | pbo = joinPath(b_, pbo, i) 306 | joinPath(b, pbo, (i+2)%4) 307 | elif flaga!=0 and flagb!=0: 308 | if flaga==2 and flagb==1: 309 | #b on a back 310 | pao = joinPath(a, pao, (i+2)%4) 311 | pao = joinPath(b, pao, i-1) 312 | pao = joinPath(b, pao, (i+2)%4) 313 | pao = joinPath(b_, pao, i) 314 | pao = joinPath(b_, pao, i-1) 315 | joinPath(a_, pao, i) 316 | elif flagb==2 and flaga==1: 317 | #a on b back 318 | pbo = joinPath(b_, pbo, i) 319 | pbo = joinPath(a_, pbo, i) 320 | pbo = joinPath(a_, pbo, (i+1)%4) 321 | pbo = joinPath(a, pbo, (i+1)%4) 322 | pbo = joinPath(a, pbo, (i+2)%4) 323 | joinPath(b, pbo, (i+2)%4) 324 | elif flaga==2 and flagb==2: 325 | #a and b bck 326 | pao = joinPath(a, pao, (i+2)%4) 327 | pao = joinPath(b, pao, (i+2)%4) 328 | pao = joinPath(b_, pao, i) 329 | joinPath(a_, pao, i) 330 | else: 331 | pao = joinPath(a, pao, (i+1)%4) 332 | pao = joinPath(a, pao, (i+2)%4) 333 | pao = joinPath(b, pao, (i+2)%4) 334 | pao = joinPath(b, pao, i-1) 335 | pao = joinPath(b_, pao, i-1) 336 | pao = joinPath(b_, pao, i) 337 | pao = joinPath(a_, pao, i) 338 | joinPath(a_, pao, (i+1)%4) 339 | 340 | elif flaga==1 and i==0: 341 | au = nextPoint(a, 1) 342 | pau = ifInPath(au) 343 | if au!=None and pau==-1: 344 | ad = nextPoint(a_, 1) 345 | pad = ifInPath(ad) 346 | if ad!=None and pad==-1: 347 | tmpcp = {} 348 | tmpcp[a_] = {a:ad} 349 | tmpcp[ad] = {a_:au} 350 | tmpcp[au] = {ad:a} 351 | tmpcp[a] = {au:a_} 352 | pao = addInFinalPath(tmpcp) 353 | obstacles[ogrid][i] = -1 354 | obstacles[nextPoint(ogrid, 0)][1] = -1 355 | obstacles[nextPoint(ogrid, 1)][3] = -1 356 | obstacles[nextPoint(nextPoint(ogrid, 0), 1)][2] = -1 357 | 358 | tmpcp = list(tmpcp) 359 | for ij in range(4): 360 | pao = joinPath(tmpcp[ij], pao, ij) 361 | pao = joinPath(tmpcp[(ij+1)%4], pao, ij) 362 | if all(obstacles[ogrid][i]!=1 for i in range(4)): del obstacles[ogrid] 363 | 364 | # Step 8 365 | while True: 366 | flag = True 367 | for ogrid in list(obstacles): 368 | for i in range(3, -1, -1): 369 | if obstacles[ogrid][i]==1: 370 | a = actualPoint(ogrid, i) 371 | tmpflag = 0 372 | for j in range(4): 373 | npt = nextPoint(a, j) 374 | ppt = ifInPath(npt) 375 | if npt!=None and ppt==-2: tmpflag += 1 376 | elif npt!=None and ppt>=0: 377 | pnpt = findBefore(npt, ppt) 378 | finalPathf[ppt][npt][a] = finalPathf[ppt][npt][pnpt] 379 | finalPathf[ppt][npt][pnpt] = a 380 | finalPathf[ppt][a] = {npt:npt} 381 | for k in range(j+1, 4): 382 | ppt = joinPath(a, ppt, k) 383 | obstacles[ogrid][i] = -1 384 | flag = False 385 | break 386 | if tmpflag==4: obstacles[ogrid][i] = -1 387 | if all(obstacles[ogrid][i]!=1 for i in range(4)): del obstacles[ogrid] 388 | if len(obstacles)==0 or flag: break 389 | 390 | # Step 9 391 | coverage_path = [] 392 | for ind in finalPathf: 393 | i = finalPathf[ind] 394 | pvertex = list(i.keys())[0] 395 | vertex = i[pvertex][list(i[pvertex].keys())[0]] if type(i[pvertex])==dict else i[pvertex] 396 | 397 | while True: 398 | # matrix[vertex] = 127 399 | coverage_path.append(vertex) 400 | if vertex not in i or pvertex not in i[vertex]: break 401 | psvertex = pvertex 402 | pvertex = vertex 403 | vertex = i[pvertex][psvertex] 404 | del i[pvertex][psvertex] 405 | if len(i[vertex])==0: 406 | del i[vertex] 407 | return coverage_path -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/stc/stc_helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def stc_preprocess(area_map): 4 | """ 5 | Returns matrix that in the form 6 | that is read by the main algorithm. 7 | """ 8 | matrix = np.full(area_map.shape, 0, dtype=np.uint8) 9 | matrix[area_map == 0] = 255 10 | return matrix -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/wavefront/__init__.py: -------------------------------------------------------------------------------- 1 | from .wavefront import wavefront -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/wavefront/wavefront.py: -------------------------------------------------------------------------------- 1 | from .wavefront_caller import wavefront_caller 2 | from .wavefront_helpers import get_replacement_paths_l1 3 | from cpp_algorithms.common_helpers import get_random_coords 4 | from cpp_algorithms.coverage_path.pathing_helpers import splice_paths, has_isolated_areas 5 | 6 | def wavefront(area_map, start_point, center_point=None, isolated_area_check=True, 7 | use_replacements_paths=True, splice_backtrack_paths=True): 8 | """ 9 | Runs the wavefront algorithm and returns a generated path. 10 | 11 | PARAMETERS 12 | --- 13 | area_map : Area map to be covered, 2-dim numpy array. 14 | coverage region = 0 15 | obstacle region = -1 16 | 17 | start_point : Drone deployment point on the `area_map`, tuple (x,y) 18 | 19 | isolated_area_check : if True will check region for isolated areas, 20 | algorithm will return None. 21 | 22 | use_replacement_paths : if True will compute the shortest bactrack paths. 23 | 24 | splice_backtrack_paths : if True will splice in the backtrack paths into the 25 | main coverage path, if False will return a dict containing the paths 26 | 27 | 28 | RETURNS 29 | --- 30 | if splice_backtrack_paths=True: 31 | will return the entire path in the form [(x1,y1),...,(xm,ym)] 32 | else: 33 | will return a dict with: 34 | coverage_path : main coverage path contains no backtracks, will be 35 | disconnected if there are backtracks. 36 | 37 | backtrack_paths : paths used when the algorithm needs to unstuck 38 | itself to get to a new region. 39 | 40 | backtrack_starts : indices in the coverage path from where the 41 | backtrack paths start. 42 | """ 43 | if center_point is None: 44 | center_point = get_random_coords(area_map, 1)[0] 45 | 46 | if isolated_area_check and has_isolated_areas(area_map): 47 | raise ValueError("map has isolated areas") 48 | 49 | coverage_path, backtrack_paths, backtrack_starts = wavefront_caller(area_map, start_point, center_point) 50 | 51 | if use_replacements_paths: 52 | backtrack_paths = get_replacement_paths_l1(backtrack_paths, area_map) 53 | 54 | if splice_backtrack_paths: 55 | return splice_paths(coverage_path, backtrack_starts, backtrack_paths) 56 | 57 | else: 58 | return { 59 | "coverage_path" : coverage_path, 60 | "backtrack_paths": backtrack_paths, 61 | "backtrack_starts": backtrack_starts 62 | } -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/wavefront/wavefront_caller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms.dist_fill import dist_fill 3 | from cpp_algorithms.common_helpers import is_valid 4 | from .wavefront_helpers import get_next_valid, update_keeper, backtrack_subroutine 5 | 6 | def wavefront_update(keeper, obstacle): 7 | c_point = keeper["curr_point"] 8 | is_new = False 9 | next_point = get_next_valid(c_point, keeper, obstacle) 10 | 11 | if next_point is not None: 12 | # non backtrack condition. 13 | is_new = update_keeper(keeper, next_point) 14 | else: 15 | # backtrack condition, no viable point found 16 | next_point = backtrack_subroutine(keeper, obstacle) 17 | is_new = update_keeper(keeper, next_point) 18 | 19 | # is_new should always be True, 20 | # else backtrack has failed and there is an error, 21 | # or the map has locked in regions. 22 | return is_new 23 | 24 | def wavefront_follow(dist_map, start_point, obstacle): 25 | # Matrix of values visited. 26 | is_visited = np.full(dist_map.shape,False) 27 | is_visited[dist_map==-1] = True 28 | is_visited[start_point] = True 29 | 30 | # Number of points to cover. 31 | points_to_cover = dist_map.size - (dist_map == obstacle).sum() - 1 32 | 33 | keeper = { 34 | "is_backtracking":False, 35 | "curr_point": start_point, 36 | "prev_point": start_point, 37 | "coverage_path": [start_point], 38 | "backtrack_paths": [], 39 | "backtrack_starts": [], 40 | "is_visited": is_visited, 41 | "dist_map": dist_map 42 | } 43 | 44 | while points_to_cover > 0: 45 | is_new = wavefront_update(keeper, obstacle) 46 | if is_new: 47 | points_to_cover -= 1 48 | return keeper["coverage_path"], keeper["backtrack_paths"], keeper["backtrack_starts"] 49 | 50 | def wavefront_caller(area_map, start_point, center_point, obstacle=-1): 51 | """ 52 | The main wavefront algorithm. 53 | start_point, center_point : form (x,y) 54 | 55 | return : 56 | coverage_path : path followed on for coverage 57 | backtrack_paths : paths followed to get to uncovered point, 58 | subsets of coverage_path. 59 | backtrack_starts : starting indices of the backtrack paths 60 | """ 61 | assert is_valid(start_point, area_map, obstacle), "invalid start" 62 | assert is_valid(center_point, area_map, obstacle), "invalid center" 63 | 64 | dist_map = dist_fill(area_map, [center_point]) 65 | return wavefront_follow(dist_map, start_point, obstacle) 66 | -------------------------------------------------------------------------------- /cpp_algorithms/coverage_path/wavefront/wavefront_helpers.py: -------------------------------------------------------------------------------- 1 | # import is_valid 2 | from cpp_algorithms.common_helpers import is_valid 3 | from cpp_algorithms.coverage_path.pathing_helpers import get_shortest_l1_path 4 | import numpy as np 5 | 6 | def update_keeper(keeper, next_point): 7 | """ 8 | Update values of the keeper dict. 9 | """ 10 | keeper["prev_point"] = keeper["curr_point"] 11 | keeper["curr_point"] = next_point 12 | keeper["coverage_path"].append(next_point) 13 | 14 | # If everything works this conditional 15 | # should evaluate to True always 16 | if not keeper["is_visited"][next_point]: 17 | keeper["is_visited"][next_point] = True 18 | return True 19 | return False 20 | 21 | def get_next_valid(c_point, keeper, obstacle): 22 | """ 23 | Checks points (RDLU) for one with max dist 24 | and not yet visited. 25 | """ 26 | rdlu = lambda p:np.array([(p[0],p[1]+1),(p[0]+1,p[1]),\ 27 | (p[0],p[1]-1),(p[0]-1,p[1])]) 28 | max_ = -1 29 | next_point = None 30 | for possible_point in rdlu(c_point): 31 | possible_point = tuple(possible_point) 32 | """ 33 | is_valid = True if the point is within bounds and 34 | hasn't been visited previously. 35 | """ 36 | if is_valid(possible_point, keeper["is_visited"], obstacle=True): 37 | d_val = keeper["dist_map"][possible_point] 38 | if d_val > max_: 39 | max_ = d_val 40 | next_point = possible_point 41 | return next_point 42 | 43 | def backtrack_subroutine(keeper, obstacle): 44 | """ 45 | Go back the coverage path, keep adding to the backtrack 46 | path, once a viable point is reached, stop, record backtrack 47 | path and backtrack start. 48 | """ 49 | # Should be True : backtrack_start == backtrack_path[0]. 50 | backtrack_start = len(keeper["coverage_path"]) - 1 # last index of coverage_path 51 | covered_path = keeper["coverage_path"][::-1] # reversed coverage_path 52 | backtrack_path = [] 53 | 54 | # Covered points are obstacles. 55 | covered_map = keeper["dist_map"].copy() 56 | x,y = np.array(covered_path).T 57 | covered_map[x,y] = obstacle 58 | 59 | for bt_point in covered_path: 60 | backtrack_path.append(bt_point) 61 | next_point = get_next_valid(bt_point, keeper, obstacle) 62 | if next_point is not None: 63 | """ 64 | keeper["backtrack_paths"] stores all `backtrack_path` lists 65 | keeper["backtrack_starts"] stores indices where the backtracking 66 | starts in the main keeper["coverage_path"] list. 67 | all elements in a `backtrack_path` are present in the `coverage_path`. 68 | """ 69 | keeper["backtrack_paths"].append(backtrack_path) 70 | keeper["backtrack_starts"].append(backtrack_start) 71 | return next_point 72 | else: 73 | raise Exception("backtrack failed", covered_path[::-1]) 74 | 75 | def get_replacement_paths_l1(backtrack_paths, area_map, obstacle=-1): 76 | """ 77 | Calculate the shortest l1 paths from the 78 | generated backtrack paths of the wavefront algorithm. 79 | """ 80 | replacement_paths = [] 81 | for backtrack_path in backtrack_paths: 82 | start_point = backtrack_path[0] 83 | end_point = backtrack_path[-1] 84 | replacement_paths.append(get_shortest_l1_path(start_point, end_point, area_map, obstacle)) 85 | return replacement_paths 86 | -------------------------------------------------------------------------------- /cpp_algorithms/cpp.py: -------------------------------------------------------------------------------- 1 | from cpp_algorithms.darp import darp 2 | from cpp_algorithms.coverage_path.bcd import bcd 3 | from cpp_algorithms.coverage_path.stc import stc 4 | from cpp_algorithms.coverage_path.wavefront import wavefront 5 | from cpp_algorithms.common_helpers import get_drone_map 6 | from cpp_algorithms.fuel_path.fuel_path import get_fuel_paths 7 | from cpp_algorithms.fuel_path.fuel_path_helpers import splice_paths 8 | 9 | 10 | def cpp(area_map, start_points, fuel_points=None, fuel_capacity=None, online=False, epochs=300, use_flood=True, 11 | drone_speed=None, drone_coverage=None, pbar=False): 12 | """ 13 | The main coverage path caller will return coverage_paths for 14 | each of the sub areas depending on the number of `start_points`. 15 | 16 | PARAMETERS 17 | --- 18 | area_map : The area map to run the algo on. 19 | start_points : list of tuples for start points of the drone. 20 | online : use online if the area is not known by the drone (uses BCD, else STC). 21 | epochs : number of iterations to run DARP for. 22 | use_flood : uses traversable l1 distance rather than direct l1 distance. 23 | drone_speed : None if all drone speeds are the same else ratio. 24 | drone_coverage : None if all drone coverage areas are the same. 25 | pbar : If True then shows progress bar. 26 | """ 27 | n = len(start_points) 28 | if n > 1: 29 | A, _ = darp(epochs, area_map, start_points, use_flood=use_flood, 30 | pbar=pbar, drone_speed=drone_speed, drone_coverage=drone_coverage) 31 | drone_maps = [get_drone_map(A, i) for i in range(n)] 32 | else: 33 | drone_maps = [area_map] 34 | 35 | if online == "wavefront": 36 | coverage_paths = [wavefront(drone_maps[i], start_points[i]) 37 | for i in range(n)] 38 | elif online: 39 | coverage_paths = [bcd(drone_maps[i], start_points[i]) 40 | for i in range(n)] 41 | else: 42 | coverage_paths = [stc(drone_maps[i], start_points[i]) 43 | for i in range(n)] 44 | if fuel_points is not None and fuel_capacity is not None: 45 | full_paths = [] 46 | detour_idxes = [] 47 | for coverage_path in coverage_paths: 48 | # Get fuel path 49 | _, detour_idx, fuel_paths, _ = get_fuel_paths( 50 | coverage_path, area_map, fuel_points, fuel_capacity) 51 | # Splice fuel path 52 | full_path, detour_idx = splice_paths( 53 | coverage_path, fuel_paths, detour_idx) 54 | full_paths.append(full_path) 55 | detour_idxes.append(detour_idx) 56 | return coverage_paths, full_paths, detour_idxes 57 | return coverage_paths, None, None 58 | -------------------------------------------------------------------------------- /cpp_algorithms/darp/__init__.py: -------------------------------------------------------------------------------- 1 | from .darp import darp -------------------------------------------------------------------------------- /cpp_algorithms/darp/continuity.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from tqdm.auto import tqdm 3 | from skimage import measure 4 | from cpp_algorithms.constants import OB 5 | from cpp_algorithms.dist_fill import dist_fill 6 | 7 | """ 8 | Functions to calculate the continuity correction matrix. 9 | """ 10 | def get_area_indices(area, value, inv=False, obstacle=OB): 11 | """ 12 | returns indices that have value and not obstacle 13 | if inv returns indices that don't have value and obstacle 14 | """ 15 | try: 16 | value = int(value) 17 | if inv: 18 | return np.concatenate([np.where((area != value)&(area!=obstacle))]).T 19 | return np.concatenate([np.where((area == value)&(area!=obstacle))]).T 20 | except: 21 | mask = area == value[0] 22 | if inv: 23 | mask = area != value[0] 24 | for v in value[1:]: 25 | if inv: 26 | mask &= area != v 27 | else: 28 | mask |= area == v 29 | mask &= area != obstacle 30 | return np.concatenate([np.where(mask)]).T 31 | 32 | def get_flood_matrix(area_map, pbar=False, obstacle=OB): 33 | """ 34 | Returns all the matrices of distances 35 | """ 36 | points = get_area_indices(area_map, obstacle, True) 37 | flood_matrix = [] 38 | for point in tqdm(points,disable=not pbar): 39 | flood_matrix.append(dist_fill(area_map, [tuple(point)])) 40 | return np.array(flood_matrix) 41 | 42 | def get_region_dist_map(index_matrix, coords): 43 | """ 44 | Broadcasts coords along index_matrix and returns the min l1 45 | """ 46 | b_val = index_matrix.shape[0] 47 | coords = np.broadcast_to(coords, (b_val, *coords.shape)) 48 | assert coords.shape[0] == index_matrix.shape[0] and \ 49 | coords.shape[2] == index_matrix.shape[2], \ 50 | "something went wrong in broadcasting" 51 | 52 | # To change the distance function change this line. 53 | return np.abs(index_matrix - coords).sum(axis=2).min(axis=1) 54 | 55 | def get_ci(cont_map, r, q): 56 | """ 57 | cont_map : continuity map that assigns unique labels to every isolated !obstacle 58 | r : value of region with the start point 59 | c : value(s) of region without the start point 60 | """ 61 | index_matrix = np.indices(cont_map.shape).transpose(1,2,0).reshape(-1, 2)[:,None,:] 62 | r_indices = get_area_indices(cont_map, r, obstacle=0) 63 | q_indices = get_area_indices(cont_map, q, obstacle=0) 64 | 65 | min_dist_r = get_region_dist_map(index_matrix, r_indices).reshape(cont_map.shape) 66 | min_dist_q = get_region_dist_map(index_matrix, q_indices).reshape(cont_map.shape) 67 | return min_dist_r - min_dist_q 68 | 69 | def min_point_map(cont_map, value, flood_matrix, obstacle=OB): 70 | """ 71 | Returns the min value to a region. 72 | """ 73 | all_idx = get_area_indices(cont_map, obstacle, True, obstacle).T 74 | val_idx = get_area_indices(cont_map, value, obstacle=obstacle).T 75 | min_map = cont_map.copy() 76 | ax,ay = all_idx 77 | vx,vy = val_idx 78 | min_map[ax,ay] = flood_matrix[:,vx,vy].min(axis=1) 79 | return min_map 80 | 81 | def get_ci_using_flood_matrix(cont_map, r, q, flood_matrix): 82 | """ 83 | Returns Ci calculated from the flood matrix 84 | """ 85 | min_dist_r = min_point_map(cont_map, r, flood_matrix, obstacle=0) 86 | min_dist_q = min_point_map(cont_map, q, flood_matrix, obstacle=0) 87 | return min_dist_r - min_dist_q 88 | 89 | # Functions that deal with continuity. 90 | def coord_in_list(coord, list_): 91 | """ 92 | Checks if coord is present in the list exactly once. 93 | """ 94 | return (np.abs(np.array(coord) - list_).sum(axis=1) ==0).sum() == 1 95 | 96 | def continuity_check(A, n, i): 97 | """ 98 | True if all areas are continuous. 99 | A : assignment map 100 | n : number of drones 101 | """ 102 | cont_map, cont_count = measure.label(A, return_num=True, background=-1) 103 | xy = get_area_indices(A,i) 104 | x,y = xy.T 105 | values_at_xy = cont_map[x,y] 106 | not_is_cont = len(np.unique(values_at_xy)) > 1 107 | if not_is_cont: 108 | return False, xy, values_at_xy, cont_map 109 | else: 110 | return True, None, None, None 111 | 112 | def continuity_fix(A, E, i, n, mi, start_point, mask, xy, values_at_xy, cont_map, flood_matrix): 113 | """ 114 | Fix continuity when there is none. 115 | """ 116 | r = None 117 | q = None 118 | some_constant = 10**-np.log10(A.size) 119 | uniq = np.unique(values_at_xy) 120 | for v in uniq: 121 | if coord_in_list(start_point,xy[values_at_xy == v]): 122 | r = v 123 | break 124 | q = uniq[uniq!=r] 125 | if flood_matrix is None: 126 | C_i = get_ci(cont_map, r, q) 127 | else: 128 | C_i = get_ci_using_flood_matrix(cont_map, r, q, flood_matrix) 129 | 130 | C_i[mask] = 1 131 | E[i] = E[i]*(C_i * some_constant + 1) 132 | 133 | def continuity_check_fix(A, E, i, n, mi, mask, start_point, flood_matrix): 134 | """ 135 | Calculate C_i and fix if `i`th drone's areas are not contiguous. 136 | """ 137 | is_contig, xy, values_at_xy, cont_map = continuity_check(A, n, i) 138 | if is_contig: 139 | return 140 | else: 141 | continuity_fix(A, E, i, n, mi, start_point, mask, xy, values_at_xy, cont_map, flood_matrix) -------------------------------------------------------------------------------- /cpp_algorithms/darp/darp.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms.constants import OB 3 | from .iterate import iterate 4 | from .continuity import get_flood_matrix 5 | from .darp_helpers import get_evaluation_matrices, get_c 6 | from .darp_helpers import get_no_obs_count, get_coverage_ratio 7 | 8 | 9 | def darp(epochs, area_map, start_points, drone_speed=None, drone_coverage=None, use_flood=True, pbar=False, obstacle=OB): 10 | """ 11 | Runs DARP on the given area map and returns the assignement matrix. 12 | 13 | PARAMETERS 14 | --- 15 | epochs : number of iterations to iterate the loop in the iterate function for. 16 | area_map : area map to be divided 17 | start_points : start points of the drones 18 | drone_speed : speed of each drone in distance/time 19 | drone_coverage : coverage area of each drone in distance^2 20 | use_flood : whether to use the l1 flood fill distance, (takes time and a lot of RAM, more accurate) 21 | pbar : whether to show the progress bar, True – shows the pbar. 22 | 23 | RETURNS 24 | --- 25 | Assignment matrix 26 | """ 27 | n = len(start_points) 28 | c = get_c(area_map) 29 | E = get_evaluation_matrices(start_points, area_map) 30 | 31 | flood_matrix = None 32 | # Flood matrix 33 | if use_flood: 34 | flood_matrix = get_flood_matrix(area_map, pbar=pbar) 35 | 36 | if drone_coverage is None or drone_speed is None: 37 | drone_speed = np.ones(n) 38 | drone_coverage = np.ones(n) 39 | 40 | nobs_count = get_no_obs_count(area_map) 41 | coverage_ratio = get_coverage_ratio(drone_speed, drone_coverage) 42 | 43 | return iterate(epochs, start_points, E, c, nobs_count, coverage_ratio, flood_matrix, pbar=pbar) 44 | -------------------------------------------------------------------------------- /cpp_algorithms/darp/darp_helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms.dist_fill import dist_fill 3 | from cpp_algorithms.constants import OB 4 | 5 | def rmse(a,b): 6 | """ 7 | Root Mean Square Error 8 | """ 9 | return ((a - b)**2).mean()**0.5 10 | 11 | def get_obstacle_coords(area_map, obstacle=OB): 12 | """ 13 | Returns coords of obstacles. 14 | """ 15 | return np.stack(list(np.where(area_map == obstacle))).T 16 | 17 | def has_obstacles(area_map, obstacle=OB): 18 | """ 19 | True if the area map has obstacles 20 | """ 21 | return (area_map == obstacle).sum() > 0 22 | 23 | def get_evaluation_matrices(start_points, area_map): 24 | """ 25 | Returns evaluation matrices for all the drones. 26 | Which are distance maps from their start_points 27 | """ 28 | return np.float32(np.stack([dist_fill(area_map,[sp]) for sp in start_points])) 29 | 30 | def get_assignment_matrix(E, obstacle=OB): 31 | """ 32 | Assigns coord depending on evaluation matrices 33 | and returns the matrix. 34 | """ 35 | mask = (E[0] == obstacle) 36 | A = E.argmin(axis=0) 37 | A[mask] = -1 38 | return A 39 | 40 | def get_assigned_count(A, arr=True, obstacle=OB): 41 | """ 42 | A : assignment matrix 43 | return count dict 44 | """ 45 | c = {} 46 | uc = np.unique(A, return_counts=True) 47 | if not arr: 48 | for drone,count in zip(*uc): 49 | if drone==obstacle: 50 | continue 51 | else: 52 | c[drone] = count 53 | return c 54 | else: 55 | return uc[1][uc[0] != OB] 56 | 57 | def get_no_obs_count(area_map, obstacle=OB): 58 | """ 59 | Return count of points that are not obstacles 60 | """ 61 | return (area_map != obstacle).sum() 62 | 63 | def get_coverage_ratio(drone_speed, drone_coverage): 64 | """ 65 | Normalized values of the drone coverage per unit time 66 | """ 67 | coverage_per_time = (drone_coverage**0.5)*drone_speed 68 | return coverage_per_time/coverage_per_time.sum() 69 | 70 | def get_c(area_map): 71 | """ 72 | area_map : for size calculation 73 | """ 74 | learning_rate = np.log10(area_map.size) # learning_rate 75 | return 10**(-np.ceil(learning_rate)) # tunable parameter c 76 | 77 | def get_fair_share(i, assigned_count, nobs_count, coverage_ratio): 78 | """ 79 | assigned_count : number of cells assigned to each drone 80 | nobs_count : total number of cells that can be covered 81 | coverage_ratio : ratio of cells to be assigned to each drone 82 | """ 83 | try: 84 | return assigned_count[i] - np.round(coverage_ratio[i] * nobs_count) 85 | except IndexError as error: 86 | # Note : if this error is thrown something is wrong. 87 | raise IndexError("the vicissitudes of cyclic coordinate descent"\ 88 | +"\n"+ "count of assigned areas has dropped below `n`"+"\n"+repr(error)) -------------------------------------------------------------------------------- /cpp_algorithms/darp/iterate.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from tqdm.auto import tqdm 3 | 4 | from cpp_algorithms.constants import OB 5 | 6 | from .darp_helpers import get_assignment_matrix, get_assigned_count 7 | from .darp_helpers import rmse, get_fair_share 8 | from .continuity import continuity_check_fix 9 | 10 | 11 | """ 12 | loop functions 13 | """ 14 | def apply_mask_E(mask, E, obstacle=OB): 15 | E[:, mask] = obstacle 16 | 17 | def mi_step(start_points, E, m, c, nobs_count, coverage_ratio, flood_matrix, obstacle=OB): 18 | """ 19 | start_points : drone start coords on the map 20 | E : evaluation matrices for all drones 21 | m : array of m_i values 22 | c : tunable parameter 23 | nobs_count : total area that can be covered 24 | coverage_ratio : coverage ratio for all drones 25 | flood_matrix : used in Ci calculation 26 | """ 27 | n = E.shape[0] 28 | mask = E[0] == obstacle 29 | A = get_assignment_matrix(E) # E changes ∴ A changes 30 | for i in range(len(m)): 31 | start_point = start_points[i] 32 | assigned_count = get_assigned_count(A, arr=True) # Counts of cells assigned to each drone 33 | fs_i = get_fair_share(i, assigned_count, nobs_count, coverage_ratio) # Difference between optimal and assigned 34 | m[i] = 1 + c * fs_i # alter the m param 35 | E[i] = E[i] * m[i] # update evaluation matrix 36 | apply_mask_E(mask, E) 37 | A = get_assignment_matrix(E) # E changes ∴ A changes 38 | continuity_check_fix(A, E, i, n, m[i], mask, start_point, flood_matrix) 39 | return A 40 | 41 | def iterate(epochs, start_points, E, c, nobs_count, coverage_ratio, flood_matrix=None, print_stuff=False, pbar=True, obstacle=OB): 42 | """ 43 | epochs : number of iterations to iterate the loop in the iterate function for. 44 | start_points : drone start coords on the map 45 | E : initial evaluation matrix for all the drones 46 | c : tunable paramter `c` 47 | nobs_count : number of cells that can be covered 48 | coverage_ratio : ratio of coverage 49 | flood_matrix : used in Ci calculation 50 | print_stuff : print some metrics 51 | pbar : whether to show the pbar 52 | """ 53 | n = E.shape[0] 54 | m = np.ones(n, dtype=np.float32) 55 | optimal_count = np.round(coverage_ratio * nobs_count) 56 | best_A = get_assignment_matrix(E) 57 | min_loss = float('inf') 58 | losses = [] 59 | for i in tqdm(range(epochs), disable=not pbar): 60 | A = mi_step(start_points, E, m, c, nobs_count, coverage_ratio, flood_matrix) 61 | assigned_count = get_assigned_count(A) 62 | loss = rmse(assigned_count, optimal_count) 63 | losses.append(loss) 64 | if print_stuff: 65 | print(f"ASGN : {assigned_count}\nRMSE : {loss:0.2f}") 66 | if loss < min_loss: 67 | min_loss = loss 68 | best_A = A 69 | return best_A, losses 70 | -------------------------------------------------------------------------------- /cpp_algorithms/dist_fill.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from .constants import OB 3 | from .common_helpers import is_valid 4 | # from cpp_algorithms.testers.helpers import is_valid 5 | 6 | def is_valid_vectorized(coords, v_map, obstacle=True): 7 | # Bound check 8 | assert coords.shape[1] == 2 9 | h,w = v_map.shape 10 | x,y = coords.T 11 | is_within_bounds = (x >= 0) & (x < h) & (y >= 0) & (y < w) 12 | x = np.clip(x.copy(), 0, h-1) 13 | y = np.clip(y.copy(), 0, w-1) 14 | is_not_on_obstacle = (v_map[x,y] != obstacle) 15 | return is_within_bounds & is_not_on_obstacle 16 | 17 | def get_udlr(coords): 18 | udlr = np.array([[-1,0],[1,0],[0,-1],[0,1]])[None,:,:] 19 | return (coords[:,None,:] + udlr).reshape(-1,2) 20 | 21 | def dist_fill_single(area_map, fill_point): 22 | v_map = area_map == OB 23 | dist_map = area_map.copy().astype(np.int32) 24 | 25 | assert is_valid(fill_point, area_map), \ 26 | "invalid fill point" 27 | 28 | fval = 0 29 | dist_map[fill_point] = fval 30 | v_map[fill_point] = True 31 | 32 | to_visit = np.array([fill_point]) 33 | while len(to_visit) > 0: 34 | x,y = np.array(to_visit).T 35 | dist_map[x,y] = fval 36 | v_map[x,y] = True 37 | udlr = np.unique(get_udlr(to_visit),axis=0) 38 | mask = is_valid_vectorized(udlr, v_map) 39 | to_visit = udlr[mask] 40 | fval += 1 41 | return dist_map 42 | 43 | def dist_fill(area_map, fill_points): 44 | """ 45 | Returns the distance matrix. 46 | 47 | PARAMETERS 48 | --- 49 | area_map : binary map, -1=obstacle, 0=area to be mapped. 50 | points : [(x,y)] points around which to dist fill (fuel station). 51 | """ 52 | dist_maps = [] 53 | for fill_point in fill_points: 54 | dist_maps.append(dist_fill_single(area_map, fill_point)) 55 | return np.array(dist_maps).min(axis=0) -------------------------------------------------------------------------------- /cpp_algorithms/fuel_path/__init__.py: -------------------------------------------------------------------------------- 1 | from .fuel_path import get_fuel_paths 2 | from .fuel_path_helpers import splice_paths -------------------------------------------------------------------------------- /cpp_algorithms/fuel_path/fuel_path.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from cpp_algorithms.dist_fill import dist_fill 4 | from .fuel_path_helpers import get_refuel_idx, path_to_fuel 5 | 6 | def get_fuel_paths(coverage_path, area_map, fuel_points, fuel_capacity): 7 | """ 8 | Calculates detour paths to the closest fuel point with optimal 9 | fuel consumption. 10 | 11 | PARAMETERS 12 | --- 13 | coverage_path : the drone coverage path as a list of tuples 14 | [(x1,y1),...,(xm,ym)] 15 | 16 | area_map : 2 dim numpy array of the area, 17 | obstacles : -1, area to map: 0 18 | 19 | fuel_points : non obstacle points on the map which are 20 | fuel_points as a list of tuples. 21 | [(x1,y1),...,(xm,ym)] 22 | 23 | fuel_capacity : int, fuel capacity of the drone interms of 24 | 1 = time required to move to an adjacent cell. 25 | 26 | 27 | RETURNS 28 | --- 29 | fuel_dist_map : a matrix of distance to the closest fuel point. 30 | 31 | detour_indices : indices of the path list where detour for 32 | fuelling has to be taken. 33 | 34 | paths_to_refuel : list of shortest path starting at `path[detour_inces[i]]` 35 | and ending at `fuel_points[i]`. 36 | 37 | fuel_capacity : list of how much fuel is remaining, same lenght as path. 38 | """ 39 | fuel_dist_map = dist_fill(area_map, fuel_points) 40 | detour_indices, fuel_capacity = get_refuel_idx(fuel_dist_map, coverage_path, fuel_capacity) 41 | 42 | paths_to_refuel = [] 43 | for idx in detour_indices: 44 | detour_location = coverage_path[idx] 45 | path_to_refuel = path_to_fuel(fuel_dist_map, detour_location) 46 | paths_to_refuel.append(path_to_refuel) 47 | 48 | return ( 49 | fuel_dist_map, 50 | detour_indices, 51 | paths_to_refuel, 52 | fuel_capacity 53 | ) -------------------------------------------------------------------------------- /cpp_algorithms/fuel_path/fuel_path_helpers.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from copy import deepcopy 3 | from cpp_algorithms.constants import FU, OB 4 | 5 | def udlr(x,y): 6 | # Up Down Left Right 7 | return [(x,y+1),(x,y-1),(x-1,y),(x+1,y)] 8 | 9 | def get_step(dist_map, point, obs=-1): 10 | # Get smallest L1 step from point. 11 | sh = dist_map.shape 12 | min_ = float('inf') 13 | p = point 14 | for direc in udlr(*point): 15 | x,y = direc 16 | if x < 0 or y < 0 or \ 17 | x >= sh[0] or y >= sh[1]: 18 | continue 19 | val = dist_map[direc] 20 | if val < min_ and val != obs: 21 | min_ = val 22 | p = direc 23 | return p, min_ 24 | 25 | def path_to_fuel(dist_map, loc, fuel=FU, obs=OB): 26 | """ 27 | PARAMETERS 28 | --- 29 | dist_map : copy of the area_map 30 | loc : (x,y) robot location 31 | fuel : value of fuel on the `dist_map` 32 | obs : value of obstacle on the `dist_map` 33 | """ 34 | loc = tuple(loc) 35 | path = [loc] 36 | cell = dist_map[loc] 37 | while cell != fuel: 38 | loc, cell = get_step(dist_map, loc, obs) 39 | path.append(loc) 40 | return path 41 | 42 | def get_refuel_idx(dist_map, coverage_path, fuel_cap): 43 | """ 44 | PARAMETERS 45 | --- 46 | dist_map : distance map where fuel points are 0 valued, 47 | (m,n) shape nd array 48 | coverage_path : sequence of coords that traverses the `dist_map`. 49 | list of tuples, [(x1,y1),...,(xm,ym)] 50 | fuel_cap : capacity of the fuel required by the drone, int. 51 | 52 | RETURNS 53 | --- 54 | points : indices in the coverage_path where refuelling detour is taken. 55 | fcap : fuel capacity at every point in the coverage_path. 56 | """ 57 | coverage_path = np.array(coverage_path) 58 | x,y = coverage_path.T 59 | dist = dist_map.copy()[x,y] 60 | plen = dist.shape[0] 61 | fcap = np.full(dist.shape, 0) 62 | 63 | points = [] 64 | 65 | cu = 0 # coverage_path distance travelled by the drone 66 | fuel_req = 0 # initial req 0 cause starting with full tank 67 | 68 | i = 0 69 | while True: 70 | i+=1 71 | ini_fuel = (fuel_cap - fuel_req) 72 | assert ini_fuel > 0, "no fuel left to complete coverage_path" 73 | # start : how many points have previously been covered. 74 | # end : how many points can be covered with available fuel. 75 | start = cu 76 | end = cu + ini_fuel 77 | 78 | # If end index overshoots coverage_path length, 79 | # end index should be the coverage_path length (ie last position -1) 80 | if plen - end < 0: 81 | end = plen 82 | sl = slice(start, end) 83 | 84 | # Amount of distance that can be travelled by the drone 85 | # taking into account the last loc. 86 | p_seg_len = end - start 87 | fin_fuel = ini_fuel - p_seg_len 88 | 89 | # Possible fuel remaining starting index 90 | fcap[sl] = np.arange(ini_fuel, fin_fuel, -1) 91 | # if : Remaining fuel is sufficient. 92 | if ini_fuel - (plen - cu) >=0: 93 | break 94 | 95 | freq = fcap[sl] - dist[sl] 96 | try: 97 | idx = freq[freq >= 0].argmin() 98 | except ValueError: 99 | raise ValueError("coverage path can't be traversed, insufficient fuel capacity") 100 | 101 | if len(points) > 1 and points[-1] == points[-2]: 102 | raise ValueError(f"coverage path can't be traversed, insufficient fuel capacity\n stuck at coverage path index :{points[-1]}") 103 | 104 | cu += idx 105 | 106 | points.append(cu) 107 | fuel_req = dist[cu] 108 | return points, fcap 109 | 110 | def splice_paths(coverage_path, fuel_paths, detour_idx, double_center=False): 111 | """ 112 | Inserts detour path to the fuelling station into the original path 113 | and returns it along with start and end indices of the full path which. 114 | 115 | PARAMETERS 116 | --- 117 | coverage_path : the drone coverage path as a list of tuples 118 | [(x1,y1),...,(xm,ym)] 119 | 120 | fuel_paths : list of fuel paths 121 | [[(x1,y1),...,(xn,yn)],...,[...]] 122 | 123 | detour_idx : indices where the drone takes a detour from the main path. 124 | 125 | double_center : whether the fuel point coordinate should be repeated. 126 | 127 | 128 | RETURNS 129 | --- 130 | full_path : The entire path of the drone on the map. 131 | 132 | detour_start_end_idx : Indices of the full path where 133 | detour starts and ends, they are the same coords. 134 | (start, end) 135 | """ 136 | coverage_path = deepcopy(coverage_path) 137 | fuel_paths = deepcopy(fuel_paths) 138 | detour_start_end_idx = [] 139 | segments = [] 140 | last_idx = 0 141 | cumu_len = 0 142 | for idx,fuel_path in zip(detour_idx,fuel_paths): 143 | s_e = [] 144 | 145 | seg = np.array(coverage_path[last_idx:idx]) 146 | segments.append(seg) 147 | cumu_len += seg.shape[0] 148 | s_e.append(cumu_len) 149 | 150 | if double_center: 151 | splice_in = [*fuel_path,*fuel_path[::-1][:-1]] 152 | else: 153 | splice_in = [*fuel_path,*fuel_path[::-1][1:-1]] 154 | 155 | seg = np.array(splice_in) 156 | cumu_len += seg.shape[0] 157 | s_e.append(cumu_len) 158 | 159 | detour_start_end_idx.append(tuple(s_e)) 160 | segments.append(seg) 161 | last_idx = idx 162 | 163 | segments.append(coverage_path[last_idx:]) 164 | full_path = np.concatenate(segments) 165 | 166 | return full_path, detour_start_end_idx -------------------------------------------------------------------------------- /cpp_algorithms/run_coverage.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from cpp_algorithms import cpp, conversion 3 | from cpp_algorithms.constants import START, FUEL 4 | from cpp_algorithms.testers.metrics import coverage_metrics 5 | 6 | 7 | def run_coverage(geojson, altitude=None, fov=None, side=None, 8 | epochs=300, online=False, use_flood=True, 9 | fuel_capacity=None, drone_range=None, drone_speed=None, 10 | drone_coverage=None, pbar=True): 11 | """ 12 | PARAMETERS 13 | --- 14 | geojson : GeoJson returned from the front end 15 | altitude : height of the drone in meters 16 | fov : field of vision of the drone's camera in degrees 17 | online : use online if the area is not known by the drone (uses BCD, else STC). 18 | epochs : number of iterations to run DARP for. 19 | use_flood : uses traversable l1 distance rather than direct l1 distance. 20 | fuel_capacity : fuel capacity in units of drone coverage side length 21 | drone_range : range of the drone 22 | drone_speed : None if all drone speeds are the same else ratio. 23 | drone_coverage : None if all drone coverage areas are the same. 24 | pbar : Progress bar, to show or not to is the quesition. 25 | """ 26 | if side == None: 27 | try: 28 | side = altitude*np.tan(fov/(180/np.pi)) 29 | except: 30 | raise ValueError("no way to calculate coverage area") 31 | area_map, points, types, retransformer = conversion(side, geojson) 32 | 33 | fuel_points = None 34 | try: 35 | start_points = points[types.index(START)] 36 | except: 37 | raise Exception("terrible error :: no start location") 38 | 39 | try: 40 | fuel_points = points[types.index(FUEL)] 41 | except: 42 | print("no fuel points found") 43 | 44 | coverage_paths, full_paths, detour_idxes = cpp( 45 | area_map, start_points, fuel_points, fuel_capacity, online, epochs, use_flood, drone_speed, drone_coverage, pbar=pbar) 46 | if full_paths is None: 47 | full_paths = list(map(np.array, coverage_paths)) 48 | lnglat_path = list(map(retransformer, full_paths)) 49 | lnglat_path = list(map(lambda l: l.reshape(-1, 2), lnglat_path)) 50 | 51 | def cm(cp): return coverage_metrics(area_map, cp) 52 | temp_paths = [] 53 | for x in lnglat_path: 54 | temp_paths.append( 55 | list(map(lambda x: {'lat': x[1].item(), 'lng': x[0].item()}, x))) 56 | 57 | lnglat_path = temp_paths 58 | 59 | temp_paths = [] 60 | for x in full_paths: 61 | temp_paths.append( 62 | list(map(lambda x: (x[0].item(), x[1].item()), x.reshape(-1, 2)))) 63 | full_paths = temp_paths 64 | 65 | list(map(cm, coverage_paths)) 66 | 67 | cov_metrics = [] 68 | for cp in coverage_paths: 69 | temp = {} 70 | cm = coverage_metrics(area_map, cp) 71 | for k in cm: 72 | try: 73 | temp[k] = cm[k].item() 74 | except: 75 | temp[k] = cm[k] 76 | cov_metrics.append(temp) 77 | 78 | return {"fullPath": full_paths, "lnglatPath": lnglat_path, "detourIndices": detour_idxes, "coverageMetrics": cov_metrics} 79 | -------------------------------------------------------------------------------- /cpp_algorithms/testers/__init__.py: -------------------------------------------------------------------------------- 1 | from .testers import single_robot_single 2 | from .testers import single_robot_multiple -------------------------------------------------------------------------------- /cpp_algorithms/testers/display_funcs.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.animation import FuncAnimation 4 | 5 | from cpp_algorithms.common_helpers import imshow 6 | from cpp_algorithms.fuel_path import splice_paths 7 | 8 | def printer(metrics): 9 | for m in metrics: 10 | print(f"{m.rjust(20)} : {metrics[m]}") 11 | 12 | def path_show(area_map, coverage_path,fuel_paths=None, \ 13 | dist_map=None, figsize=(7,7),s=7, alpha=0.5, 14 | cmap="Greys_r"): 15 | """ 16 | Displays `area_map` with the path 17 | taken by the drone overlaid ontop of 18 | it. 19 | 20 | PARAMETERS 21 | --- 22 | area_map : -1 (obstacle), 0 valued 2-dim ndarray 23 | coverage_path : area coverage path [(x1,y1),..., (xm,ym)] 24 | fuel_paths : detour paths to refuel [[(x1,y1),..., (xn,yn)],...,[...]] 25 | dist_map : path showing dist gradients to the fuel points 26 | s : path marker size 27 | alpha : path marker alpha 28 | """ 29 | x,y = np.array(coverage_path).T 30 | if dist_map is not None: 31 | imshow(dist_map,figsize,cmap=cmap) 32 | else: 33 | imshow(area_map,figsize,cmap=cmap) 34 | sd = 3 35 | ad = 0.2 36 | plt.scatter(y,x,color="orange", s=s,alpha=alpha) 37 | plt.scatter(y[0],x[0],color="green", s=s+sd,alpha=alpha+ad) 38 | plt.scatter(y[-1],x[-1],color="red", s=s+sd,alpha=alpha+ad) 39 | 40 | if fuel_paths is not None: 41 | for fuel_path in fuel_paths: 42 | x,y = np.array(fuel_path).T 43 | plt.scatter(y,x,color="lightblue", s=s,alpha=alpha) 44 | plt.scatter(y[0],x[0],color="yellow", s=s+sd,alpha=alpha+ad) 45 | plt.scatter(y[-1],x[-1],color="cyan", s=s+sd,alpha=alpha+ad) 46 | 47 | 48 | def path_animate(values, interval=10, repeat=False): 49 | disp_map = values["dist_map"] 50 | if disp_map is None: 51 | disp_map = values["area_map"] 52 | 53 | coverage_path = values['coverage_path'] 54 | start_point = values["start_point"] 55 | end_point = values["end_point"] 56 | 57 | fuel_points = values["fuel_points"] 58 | if fuel_points is not None: 59 | fuel_paths = values["fuel_paths"] 60 | detour_idx = values["detour_idx"] 61 | full_path,detour_se_idx = splice_paths(coverage_path, fuel_paths, detour_idx) 62 | det_se_flat = np.array(detour_se_idx).flatten() 63 | else: 64 | full_path = np.array(coverage_path) 65 | detour_se_idx = [] 66 | det_se_flat = [] 67 | 68 | frames = full_path.shape[0] 69 | 70 | fig, ax = plt.subplots(figsize=(8,8)) 71 | ax.imshow(disp_map, interpolation="none", cmap="cividis") 72 | ax.axis('off') 73 | 74 | x,y = np.array(full_path).T 75 | 76 | features = { 77 | "c_count":-1, 78 | "f_count":0, 79 | "f_num": -1, 80 | "fuelling": False, 81 | "text" : ax.text(0,-2," - "), 82 | "c_path" : ax.plot([],[], color="#FF69B4", alpha=0.8)[0], 83 | "f_path" : [ax.plot([],[], color="#BBDEFB", alpha=0.8)\ 84 | [0] for _ in range(len(detour_se_idx))] 85 | } 86 | 87 | if fuel_points is not None: 88 | f,g = np.array(fuel_points).T 89 | ax.scatter(g,f, s=10, color="#FFF") 90 | ax.scatter(start_point[1],start_point[0], s=12, color="#b2ebf2") 91 | ax.scatter(end_point[1],end_point[0], s=12, color="#FF3D00") 92 | 93 | def next_frame(i): 94 | is_fu = features["fuelling"] 95 | if not is_fu: 96 | features["c_count"] = features["c_count"] + 1 97 | else: 98 | features["f_count"] = features["f_count"] + 1 99 | 100 | if i in det_se_flat: 101 | if not is_fu: 102 | ax.scatter(y[i],x[i], s=12, color="#ffca28") 103 | features["fuelling"] = True 104 | features["f_num"] += 1 105 | else: 106 | features["fuelling"] = False 107 | 108 | mode = "fuelling" if is_fu else "mapping" 109 | features["text"].set_text(f"{mode} :"+\ 110 | f": total path left : {frames - i - 1} :"+\ 111 | f": f_count :{features['f_count']} :"+\ 112 | f": c_count :{features['c_count']} :"+\ 113 | f": num_refuels:{features['f_num']+1}") 114 | 115 | if features["fuelling"]: 116 | f_num = features["f_num"] 117 | y_,x_ = features["f_path"][f_num].get_data() 118 | x_ = [*x_, x[i]] 119 | y_ = [*y_, y[i]] 120 | features["f_path"][f_num].set_data(y_,x_) 121 | else: 122 | y_,x_ = features["c_path"].get_data() 123 | x_ = [*x_, x[i]] 124 | y_ = [*y_, y[i]] 125 | features["c_path"].set_data(y_,x_) 126 | return FuncAnimation(fig, next_frame, frames=frames, interval=interval, repeat=repeat); -------------------------------------------------------------------------------- /cpp_algorithms/testers/metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def coverage_metrics(area_map, path): 4 | """ 5 | Returns a dict of metrics pertaining to area coverage. 6 | 7 | PARAMETERS 8 | --- 9 | area_map : -1 (obstacle), 0 valued 2 dim ndarray 10 | path : [(x1,y1),..., (xm,ym)] 11 | """ 12 | path = np.array(path).T 13 | assert path.shape[0] == 2 14 | x,y = path 15 | 16 | p_len = path.shape[1] 17 | obs_map = area_map == -1 18 | obs_points = obs_map.sum() 19 | total_points_nobs = (~obs_map).sum() 20 | 21 | vis_map = obs_map.copy() 22 | vis_map[x,y] = True 23 | vis_points = vis_map.sum() - obs_points 24 | 25 | coverage = vis_points/total_points_nobs 26 | redundancy = p_len/vis_points - 1 27 | 28 | return { 29 | "points_to_visit":total_points_nobs, 30 | "obstacle_points":obs_points, 31 | "points_visited":vis_points, 32 | "coverage_path_len":p_len, 33 | "coverage":coverage, 34 | "redundancy":redundancy, 35 | "area_shape":area_map.shape 36 | } 37 | 38 | def fuel_metrics(fuel_paths, fuel_capacity, full_path, area_map): 39 | """ 40 | TODO : Need to complete this function, add more metrics. 41 | """ 42 | full_path_metrics = coverage_metrics(area_map, full_path) 43 | fp_len = np.sum([len(fp) for fp in fuel_paths])*2 44 | return { 45 | "refuels" : len(fuel_paths), 46 | "fuel_path_len": fp_len, 47 | "fuel_capacity": fuel_capacity, 48 | "total_redundancy": full_path_metrics["redundancy"], 49 | } -------------------------------------------------------------------------------- /cpp_algorithms/testers/testers.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import pandas as pd 4 | from tqdm.auto import tqdm 5 | 6 | from cpp_algorithms.dist_fill import dist_fill 7 | from cpp_algorithms.fuel_path import get_fuel_paths 8 | from cpp_algorithms.fuel_path import splice_paths 9 | from cpp_algorithms.common_helpers import get_random_coords 10 | 11 | from .metrics import coverage_metrics, fuel_metrics 12 | from .display_funcs import path_show, path_animate, printer 13 | 14 | # Fuel capacity distance multiplier 15 | FC_DIST_MULTIPLIER = 5 16 | 17 | """ 18 | Test function to run tests on multiple image maps all at once. 19 | """ 20 | 21 | def single_robot_multiple(cpp_algo, area_maps, no_end=True, fuel_paths=False, start_point=None, center_point=None): 22 | """ 23 | Returns a DataFrame of test Results. 24 | Tests are run with randomly generated points for start, end and fuel. 25 | 26 | PARAMETERS 27 | --- 28 | cpp_algo : the algorithm used to compute the path. 29 | function signature: 30 | no_end=True : `cpp_algo(area_map, start_point)` 31 | no_end=False : `cpp_algo(area_map, start_point, end_point)` 32 | area_maps : list of area_map on which the path has to be calculated. 33 | no_end : if the `cpp_algo` requires an end point then should be true. 34 | fuel_paths : whether to calculate the fuel paths. 35 | start_point : to be used only when the same map is used for randomness 36 | dependence checks. 37 | center_point : for testing wavefront 38 | """ 39 | 40 | results = [] 41 | is_start_none = start_point is None 42 | for i, area_map in tqdm(enumerate(area_maps), total=len(area_maps)): 43 | t_metrics = {} 44 | f_metrics = {} 45 | c_metrics = {} 46 | if is_start_none: 47 | start_point = get_random_coords(area_map, 1)[0] 48 | end_point = get_random_coords(area_map, 1)[0] 49 | 50 | # Coverage Path calculation. 51 | t = time.time() 52 | try: 53 | if no_end and center_point is not None: 54 | coverage_path = cpp_algo( 55 | area_map, start_point, center_point=center_point) 56 | if no_end: 57 | coverage_path = cpp_algo(area_map, start_point) 58 | else: 59 | coverage_path = cpp_algo(area_map, start_point, end_point) 60 | t_metrics["success"] = True 61 | t_metrics["cp_compute_time"] = time.time() - t 62 | c_metrics = coverage_metrics(area_map, coverage_path) 63 | except: 64 | t_metrics["success"] = False 65 | 66 | # Fuel Path calculation. 67 | if fuel_paths == True and t_metrics["success"]: 68 | n_fuel_stations = np.random.randint(1, 5) 69 | fuel_points = get_random_coords(area_map, n_fuel_stations) 70 | dm = dist_fill(area_map, fuel_points) 71 | temp = dm.max() * FC_DIST_MULTIPLIER 72 | fuel_capacity = np.random.randint(int(0.8*temp), int(1.2*temp)) 73 | 74 | t = time.time() 75 | dist_map, detour_idx, fuel_paths_, _ = get_fuel_paths(coverage_path, 76 | area_map, fuel_points, fuel_capacity) 77 | full_path, _ = splice_paths(coverage_path, fuel_paths_, detour_idx) 78 | t_metrics["fp_compute_time"] = time.time() - t 79 | f_metrics = fuel_metrics( 80 | fuel_paths_, fuel_capacity, full_path, area_map) 81 | f_metrics["max_dist_fuel"] = dist_map.max() 82 | 83 | results.append({ 84 | **t_metrics, 85 | **c_metrics, 86 | **f_metrics 87 | }) 88 | return pd.DataFrame(results) 89 | 90 | 91 | """ 92 | Test Function to run tests on a single test map and show the results 93 | """ 94 | 95 | 96 | def single_robot_single(cpp_algo, area_map, no_end=True, fuel_paths=False, 97 | start_point=None, end_point=None, fuel_points=None, 98 | fuel_capacity=None, fp_count=None, animate=False, 99 | interval=10, repeat=False, printm=True, show_paths=True, 100 | figsize=(7, 7), cmap="Greys_r"): 101 | """ 102 | Returns a DataFrame of test Results. 103 | Tests are run with randomly generated points for start, end and fuel. 104 | 105 | PARAMETERS 106 | --- 107 | cpp_algo : the algorithm used to compute the path. 108 | function signature: 109 | no_end=True : `cpp_algo(area_map, start_point)` 110 | no_end=False : `cpp_algo(area_map, start_point, end_point)` 111 | area_map : list of area_map on which the path has to be calculated. 112 | no_end : if the `cpp_algo` requires an end point then should be true. 113 | fuel_paths : whether to calculate the fuel paths. 114 | 115 | -- 116 | If the following values are none random values are used: 117 | start_point : (x,y) starting point of the drone. 118 | end_point : (x,y) end point of the drone. 119 | fuel_points : [(x,y),...,(xn,yn)] fuel points. 120 | fuel_capacity : drone's fuel capacity. 121 | fp_count : If fuel_points is None, how many fuel points to generate. 122 | -- 123 | 124 | animate : whether to animate the motion of the drone. 125 | interval : interval in ms between animation frames. 126 | repeat : whether to repeat the animation. 127 | printm : whether to print the drone metrics. 128 | show_paths : show the map. 129 | fig_size : size of the matplotlib figure to display the animation 130 | or area map. 131 | cmap : Which matplotlib color map to use. 132 | """ 133 | 134 | t_metrics = {} 135 | f_metrics = {} 136 | c_metrics = {} 137 | 138 | fuel_paths_ = None 139 | dist_map = None 140 | detour_idx = None 141 | fuel_capacity_list = None 142 | coverage_path = None 143 | start_point = get_random_coords( 144 | area_map, 1)[0] if start_point is None else start_point 145 | end_point = get_random_coords( 146 | area_map, 1)[0] if end_point is None else end_point 147 | 148 | # Coverage Path calculation. 149 | t = time.time() 150 | try: 151 | if no_end: 152 | coverage_path = cpp_algo(area_map, start_point) 153 | else: 154 | coverage_path = cpp_algo(area_map, start_point, end_point) 155 | t_metrics["success"] = True 156 | t_metrics["cp_compute_time"] = time.time() - t 157 | c_metrics = coverage_metrics(area_map, coverage_path) 158 | except: 159 | t_metrics["success"] = False 160 | 161 | if fuel_paths == True and t_metrics["success"]: 162 | if fuel_points is None: 163 | fp_count = np.random.randint( 164 | 1, 5) if fp_count is None else fp_count 165 | fuel_points = get_random_coords(area_map, fp_count) 166 | 167 | if fuel_capacity is None: 168 | dist_map = dist_fill(area_map, fuel_points) 169 | temp = dist_map.max() * FC_DIST_MULTIPLIER 170 | fuel_capacity = np.random.randint(int(0.8*temp), int(1.2*temp)) 171 | 172 | t = time.time() 173 | dist_map, detour_idx, fuel_paths_, fuel_capacity_list = get_fuel_paths(coverage_path, 174 | area_map, fuel_points, fuel_capacity) 175 | 176 | full_path, _ = splice_paths(coverage_path, fuel_paths_, detour_idx) 177 | t_metrics["fp_compute_time"] = time.time() - t 178 | f_metrics = fuel_metrics( 179 | fuel_paths_, fuel_capacity, full_path, area_map) 180 | f_metrics["max_dist_fuel"] = dist_map.max() 181 | 182 | metrics = { 183 | **t_metrics, 184 | **c_metrics, 185 | **f_metrics, 186 | } 187 | 188 | values = { 189 | "area_map": area_map, 190 | "coverage_path": coverage_path, 191 | "fuel_paths": fuel_paths_, 192 | "detour_idx": detour_idx, 193 | "dist_map": dist_map, 194 | "fuel_points": fuel_points, 195 | "fuel_capacity": fuel_capacity, 196 | "start_point": start_point, 197 | "end_point": end_point, 198 | "fuel_capacity_list": fuel_capacity_list 199 | } 200 | if coverage_path is None: 201 | return metrics, values 202 | 203 | if not animate and show_paths: 204 | path_show(area_map, coverage_path, fuel_paths_, 205 | dist_map, figsize=figsize, cmap=cmap) 206 | 207 | if animate: 208 | values["__anim__"] = path_animate(values, interval, repeat) 209 | 210 | if printm: 211 | printer(metrics) 212 | 213 | return metrics, values 214 | -------------------------------------------------------------------------------- /demo_media/bous.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/bous.png -------------------------------------------------------------------------------- /demo_media/darp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/darp.png -------------------------------------------------------------------------------- /demo_media/kmstcr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/kmstcr.png -------------------------------------------------------------------------------- /demo_media/kmstcsh.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/kmstcsh.png -------------------------------------------------------------------------------- /demo_media/sptc.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/sptc.png -------------------------------------------------------------------------------- /demo_media/stcfuel.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/stcfuel.gif -------------------------------------------------------------------------------- /demo_media/stctests.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/stctests.png -------------------------------------------------------------------------------- /demo_media/wavf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/demo_media/wavf.png -------------------------------------------------------------------------------- /test_maps/caves_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/caves_0.png -------------------------------------------------------------------------------- /test_maps/caves_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/caves_1.png -------------------------------------------------------------------------------- /test_maps/center_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/center_0.png -------------------------------------------------------------------------------- /test_maps/center_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/center_1.png -------------------------------------------------------------------------------- /test_maps/comb_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_0.png -------------------------------------------------------------------------------- /test_maps/comb_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_1.png -------------------------------------------------------------------------------- /test_maps/comb_11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_11.png -------------------------------------------------------------------------------- /test_maps/comb_12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_12.png -------------------------------------------------------------------------------- /test_maps/comb_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_2.png -------------------------------------------------------------------------------- /test_maps/comb_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_3.png -------------------------------------------------------------------------------- /test_maps/comb_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_4.png -------------------------------------------------------------------------------- /test_maps/comb_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_5.png -------------------------------------------------------------------------------- /test_maps/comb_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_6.png -------------------------------------------------------------------------------- /test_maps/comb_7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_7.png -------------------------------------------------------------------------------- /test_maps/comb_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_8.png -------------------------------------------------------------------------------- /test_maps/comb_9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/comb_9.png -------------------------------------------------------------------------------- /test_maps/corners_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/corners_0.png -------------------------------------------------------------------------------- /test_maps/pipes_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/pipes_0.png -------------------------------------------------------------------------------- /test_maps/pipes_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/pipes_1.png -------------------------------------------------------------------------------- /test_maps/pipes_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_maps/pipes_2.png -------------------------------------------------------------------------------- /test_notebooks/BoustrophedonTests.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Boustrophedon Formulation and Tests" 8 | ] 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": 2, 13 | "metadata": { 14 | "ExecuteTime": { 15 | "end_time": "2020-07-31T17:35:10.663236Z", 16 | "start_time": "2020-07-31T17:35:10.370677Z" 17 | } 18 | }, 19 | "outputs": [ 20 | { 21 | "name": "stderr", 22 | "output_type": "stream", 23 | "text": [ 24 | "/Users/alan/opt/anaconda3/envs/data_sci/lib/python3.7/importlib/_bootstrap.py:219: RuntimeWarning: numpy.ufunc size changed, may indicate binary incompatibility. Expected 192 from C header, got 216 from PyObject\n", 25 | " return f(*args, **kwds)\n", 26 | "/Users/alan/opt/anaconda3/envs/data_sci/lib/python3.7/importlib/_bootstrap.py:219: RuntimeWarning: numpy.ufunc size changed, may indicate binary incompatibility. Expected 192 from C header, got 216 from PyObject\n", 27 | " return f(*args, **kwds)\n", 28 | "/Users/alan/opt/anaconda3/envs/data_sci/lib/python3.7/importlib/_bootstrap.py:219: RuntimeWarning: numpy.ufunc size changed, may indicate binary incompatibility. Expected 192 from C header, got 216 from PyObject\n", 29 | " return f(*args, **kwds)\n" 30 | ] 31 | } 32 | ], 33 | "source": [ 34 | "import math\n", 35 | "import time\n", 36 | "import numpy as np\n", 37 | "from tqdm.auto import tqdm\n", 38 | "from warnings import warn" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": 1, 44 | "metadata": { 45 | "ExecuteTime": { 46 | "end_time": "2020-07-31T17:35:08.075180Z", 47 | "start_time": "2020-07-31T17:35:08.069735Z" 48 | } 49 | }, 50 | "outputs": [], 51 | "source": [ 52 | "from cpp_algorithms.common_helpers import is_valid, adjacency_test\n", 53 | "from cpp_algorithms.common_helpers import imshow, imshow_scatter\n", 54 | "from cpp_algorithms.common_helpers import get_all_area_maps, get_random_coords, get_area_map\n", 55 | "from cpp_algorithms.testers.metrics import coverage_metrics\n", 56 | "from cpp_algorithms.testers.display_funcs import printer" 57 | ] 58 | }, 59 | { 60 | "cell_type": "markdown", 61 | "metadata": {}, 62 | "source": [ 63 | "# A - Star\n", 64 | "(untouched)" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": 4, 70 | "metadata": { 71 | "ExecuteTime": { 72 | "end_time": "2020-07-28T19:57:25.600626Z", 73 | "start_time": "2020-07-28T19:57:25.592921Z" 74 | } 75 | }, 76 | "outputs": [], 77 | "source": [ 78 | "def heuristic(start, goal):\n", 79 | " #Use Chebyshev distance heuristic if we can move one square either\n", 80 | " #adjacent or diagonal\n", 81 | " D = 1\n", 82 | " D2 = 1\n", 83 | " dx = abs(start[0] - goal[0])\n", 84 | " dy = abs(start[1] - goal[1])\n", 85 | " return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)\n", 86 | " \n", 87 | "def get_vertex_neighbours(pos, diameter, width, height):\n", 88 | " n = []\n", 89 | " #Moves allow link a chess king\n", 90 | " for dx, dy in [(diameter,0),(-diameter,0),(0,diameter),(0,-diameter)]:\n", 91 | " x2 = pos[0] + dx\n", 92 | " y2 = pos[1] + dy\n", 93 | " if x2 < 0 or x2 > width-1 or y2 < 0 or y2 > height-1:\n", 94 | " continue\n", 95 | " n.append((x2, y2))\n", 96 | " return n\n", 97 | " " 98 | ] 99 | }, 100 | { 101 | "cell_type": "code", 102 | "execution_count": 5, 103 | "metadata": { 104 | "ExecuteTime": { 105 | "end_time": "2020-07-28T19:57:25.613460Z", 106 | "start_time": "2020-07-28T19:57:25.603462Z" 107 | } 108 | }, 109 | "outputs": [], 110 | "source": [ 111 | "def AStarSearch(start, end, graph, diameter, width, height):\n", 112 | " \n", 113 | " G = {} #Actual movement cost to each position from the start position\n", 114 | " F = {} #Estimated movement cost of start to end going via this position\n", 115 | " \n", 116 | " #Initialize starting values\n", 117 | " G[start] = 0 \n", 118 | " F[start] = heuristic(start, end)\n", 119 | " \n", 120 | " closedVertices = set()\n", 121 | " openVertices = set([start])\n", 122 | " cameFrom = {}\n", 123 | " \n", 124 | " # Adding a stop condition\n", 125 | " outer_iterations = 0\n", 126 | " max_iterations = (len(graph[0]) * len(graph) // 2)\n", 127 | " \n", 128 | " while len(openVertices) > 0:\n", 129 | " outer_iterations += 1\n", 130 | "\n", 131 | " if outer_iterations > max_iterations:\n", 132 | " warn(\"number of iterations has exceeded max iterations\")\n", 133 | " \n", 134 | " #Get the vertex in the open list with the lowest F score\n", 135 | " current = None\n", 136 | " currentFscore = None\n", 137 | " for pos in openVertices:\n", 138 | " if current is None or F[pos] < currentFscore:\n", 139 | " currentFscore = F[pos]\n", 140 | " current = pos\n", 141 | " \n", 142 | " #Check if we have reached the goal\n", 143 | " if current == end:\n", 144 | " #Retrace our route backward\n", 145 | " path = [current]\n", 146 | " while current in cameFrom:\n", 147 | " current = cameFrom[current]\n", 148 | " path.append(current)\n", 149 | " path.reverse()\n", 150 | " return path #Done!\n", 151 | " \n", 152 | " #Mark the current vertex as closed\n", 153 | " openVertices.remove(current)\n", 154 | " closedVertices.add(current)\n", 155 | " \n", 156 | " #Update scores for vertices near the current position\n", 157 | " for neighbour in get_vertex_neighbours(current, diameter, width, height):\n", 158 | " if neighbour in closedVertices: \n", 159 | " continue #We have already processed this node exhaustively\n", 160 | " x=neighbour[0]\n", 161 | " y=neighbour[1]\n", 162 | " if graph[x][y]!=150:\n", 163 | " continue\n", 164 | " else:\n", 165 | " candidateG = G[current] + 1\n", 166 | " \n", 167 | " if neighbour not in openVertices:\n", 168 | " openVertices.add(neighbour) #Discovered a new vertex\n", 169 | " \n", 170 | " elif candidateG >= G[neighbour]:\n", 171 | " continue #This G score is worse than previously found\n", 172 | " \n", 173 | " #Adopt this G score\n", 174 | " cameFrom[neighbour] = current\n", 175 | " G[neighbour] = candidateG\n", 176 | " H = heuristic(neighbour, end)\n", 177 | " F[neighbour] = G[neighbour] + H\n", 178 | " \n", 179 | " raise RuntimeError(\"A* failed to find a solution\")" 180 | ] 181 | }, 182 | { 183 | "cell_type": "code", 184 | "execution_count": 6, 185 | "metadata": { 186 | "ExecuteTime": { 187 | "end_time": "2020-07-28T19:57:25.821071Z", 188 | "start_time": "2020-07-28T19:57:25.814672Z" 189 | } 190 | }, 191 | "outputs": [], 192 | "source": [ 193 | "def AstarPath(M, Memory, path):\n", 194 | " for q in path:\n", 195 | " x=q[0]\n", 196 | " y=q[1]\n", 197 | " Memory.append((x, y))" 198 | ] 199 | }, 200 | { 201 | "cell_type": "markdown", 202 | "metadata": {}, 203 | "source": [ 204 | "# Boustrophedon and caller" 205 | ] 206 | }, 207 | { 208 | "cell_type": "code", 209 | "execution_count": 7, 210 | "metadata": { 211 | "ExecuteTime": { 212 | "end_time": "2020-07-28T19:57:27.095795Z", 213 | "start_time": "2020-07-28T19:57:27.073119Z" 214 | } 215 | }, 216 | "outputs": [], 217 | "source": [ 218 | "def is_valid_vectorized(coords, matrix):\n", 219 | " # Bound check\n", 220 | " assert coords.shape[1] == 2\n", 221 | " h,w = matrix.shape\n", 222 | " x,y = coords.T\n", 223 | " is_within_bounds = (x >= 0) & (x < h) & (y >= 0) & (y < w)\n", 224 | " x = np.clip(x.copy(), 0, h-1)\n", 225 | " y = np.clip(y.copy(), 0, w-1)\n", 226 | " is_not_on_obstacle = (matrix[x,y] != 0) & (matrix[x,y] != 150)\n", 227 | " return is_within_bounds & is_not_on_obstacle\n", 228 | "\n", 229 | "def backtracking_list(memory, _, matrix, x, y):\n", 230 | " bt_cond_points = {\n", 231 | " \"r\": lambda p: p + np.array([[0,1]]), # right\n", 232 | " \"tr\": lambda p: p + np.array([[-1,1]]), # top-right\n", 233 | " \"t\": lambda p: p + np.array([[-1,0]]), # top\n", 234 | " \"tl\": lambda p: p + np.array([[-1,-1]]), # top-left\n", 235 | " \"l\": lambda p: p + np.array([[0,-1]]), # left\n", 236 | " \"bl\": lambda p: p + np.array([[1,-1]]), # bottom-left\n", 237 | " \"b\": lambda p: p + np.array([[1,0]]), # bottom\n", 238 | " \"br\": lambda p: p + np.array([[1,1]]), # bottom-right\n", 239 | " }\n", 240 | " memory_ = np.array(memory)\n", 241 | " assert memory_.shape[1] == 2, \"you've messed up something\"\n", 242 | " \n", 243 | " eight_di = {k: bt_cond_points[k](memory_) for k in bt_cond_points}\n", 244 | " is_valid_eight = {k: is_valid_vectorized(eight_di[k], matrix) for k in eight_di}\n", 245 | " cond_a = np.int0(is_valid_eight[\"r\"] & ~is_valid_eight[\"br\"])\n", 246 | " cond_b = np.int0(is_valid_eight[\"r\"] & ~is_valid_eight[\"tr\"])\n", 247 | " cond_c = np.int0(is_valid_eight[\"l\"] & ~is_valid_eight[\"bl\"])\n", 248 | " cond_d = np.int0(is_valid_eight[\"l\"] & ~is_valid_eight[\"tl\"])\n", 249 | " cond_e = np.int0(is_valid_eight[\"b\"] & ~is_valid_eight[\"bl\"])\n", 250 | " cond_f = np.int0(is_valid_eight[\"b\"] & ~is_valid_eight[\"br\"])\n", 251 | " μ_of_s = (cond_a + cond_b+ cond_c + cond_d + cond_e + cond_f)\n", 252 | " \n", 253 | " backtrack_points = memory_[μ_of_s > 0]\n", 254 | " if backtrack_points.shape[0] == 0:\n", 255 | " backtrack_points = memory_[is_valid_eight[\"r\"] | is_valid_eight[\"l\"] |\n", 256 | " is_valid_eight[\"t\"] | is_valid_eight[\"b\"]]\n", 257 | " \n", 258 | " if backtrack_points.shape[0] == 0:\n", 259 | " return (0,0), True\n", 260 | " else:\n", 261 | " closest_point_idx = ((backtrack_points - np.array([x,y]))**2).sum(axis = 1).argmin()\n", 262 | " return tuple(backtrack_points[closest_point_idx]), False" 263 | ] 264 | }, 265 | { 266 | "cell_type": "code", 267 | "execution_count": 79, 268 | "metadata": { 269 | "ExecuteTime": { 270 | "end_time": "2020-07-28T18:28:04.759309Z", 271 | "start_time": "2020-07-28T18:28:04.668414Z" 272 | }, 273 | "scrolled": true 274 | }, 275 | "outputs": [ 276 | { 277 | "name": "stdout", 278 | "output_type": "stream", 279 | "text": [ 280 | "1.72 ms ± 388 µs per loop (mean ± std. dev. of 5 runs, 10 loops each)\n" 281 | ] 282 | } 283 | ], 284 | "source": [ 285 | "%timeit -n 10 -r 5 bt_point = backtracking_list(coverage_path, None,area_map, 0,0)" 286 | ] 287 | }, 288 | { 289 | "cell_type": "code", 290 | "execution_count": 9, 291 | "metadata": { 292 | "ExecuteTime": { 293 | "end_time": "2020-07-28T18:32:34.950510Z", 294 | "start_time": "2020-07-28T18:32:34.937690Z" 295 | } 296 | }, 297 | "outputs": [], 298 | "source": [ 299 | "def backtracking_list_nv(memory, _, matrix, x, y):\n", 300 | " # Non vectorised implementation, runs faster; contig array alloc takes time!?\n", 301 | " bt_cond_points = {\n", 302 | " \"r\": lambda x, y: (x + (+0), y + (+1)), # right\n", 303 | " \"tr\": lambda x, y: (x + (-1), y + (+1)), # top-right\n", 304 | " \"t\": lambda x, y: (x + (-1), y + (+0)), # top\n", 305 | " \"tl\": lambda x, y: (x + (-1), y + (-1)), # top-left\n", 306 | " \"l\": lambda x, y: (x + (+0), y + (-1)), # left\n", 307 | " \"bl\": lambda x, y: (x + (+1), y + (-1)), # bottom-left\n", 308 | " \"b\": lambda x, y: (x + (+1), y + (+0)), # bottom\n", 309 | " \"br\": lambda x, y: (x + (+1), y + (+1)) # bottom-right\n", 310 | " }\n", 311 | " \n", 312 | " μ_of_s = []; \n", 313 | " for point in memory:\n", 314 | " av = { k:is_valid(bt_cond_points[k](*point), matrix, [0,150]) for k in bt_cond_points }\n", 315 | " μ_of_s.append(int(av[\"r\"] and not av[\"br\"]) + int(av[\"r\"] and not av[\"tr\"]) + \n", 316 | " int(av[\"l\"] and not av[\"bl\"]) + int(av[\"l\"] and not av[\"tl\"]) + \n", 317 | " int(av[\"b\"] and not av[\"bl\"]) + int(av[\"b\"] and not av[\"br\"]))\n", 318 | " μ_of_s = np.array(μ_of_s); # backup = np.array(backup);\n", 319 | " memory_ = np.array(memory)\n", 320 | " \n", 321 | " backtrack_points = memory_[μ_of_s > 0]\n", 322 | " if backtrack_points.shape[0] == 0:\n", 323 | " return (0,0), True\n", 324 | " else:\n", 325 | " closest_point_idx = ((backtrack_points - np.array([x,y]))**2).sum(axis = 1).argmin()\n", 326 | " return tuple(backtrack_points[closest_point_idx]), False" 327 | ] 328 | }, 329 | { 330 | "cell_type": "code", 331 | "execution_count": 107, 332 | "metadata": { 333 | "ExecuteTime": { 334 | "end_time": "2020-07-28T18:31:42.799842Z", 335 | "start_time": "2020-07-28T18:31:38.923013Z" 336 | } 337 | }, 338 | "outputs": [ 339 | { 340 | "name": "stdout", 341 | "output_type": "stream", 342 | "text": [ 343 | "77.5 ms ± 3.05 ms per loop (mean ± std. dev. of 5 runs, 10 loops each)\n" 344 | ] 345 | } 346 | ], 347 | "source": [ 348 | "%timeit -n 10 -r 5 _ = backtracking_list(coverage_path, None,area_map, 0,0)" 349 | ] 350 | }, 351 | { 352 | "cell_type": "code", 353 | "execution_count": 8, 354 | "metadata": { 355 | "ExecuteTime": { 356 | "end_time": "2020-07-28T19:57:32.566578Z", 357 | "start_time": "2020-07-28T19:57:32.558475Z" 358 | } 359 | }, 360 | "outputs": [], 361 | "source": [ 362 | "def visit(matrix, x, y, memory):\n", 363 | " matrix[(x,y)] = 150 # 150 == visited\n", 364 | " memory.append((x, y))\n", 365 | " return x,y\n", 366 | "\n", 367 | "def boustrophedon(matrix, diameter, x, y, memory):\n", 368 | " # TODO :: Variable diameter support\n", 369 | " udlr = {\n", 370 | " \"u\": lambda x,y : (x-diameter,y),\n", 371 | " \"d\": lambda x,y : (x+diameter,y),\n", 372 | " \"l\": lambda x,y : (x,y-diameter),\n", 373 | " \"r\": lambda x,y : (x,y+diameter)\n", 374 | " }\n", 375 | " u = \"u\";d = \"d\";r = \"r\";l = \"l\"\n", 376 | " visit(matrix, x, y, memory)\n", 377 | " \n", 378 | " while True:\n", 379 | " dir_ = [u,d,r,l]\n", 380 | " while len(dir_) > 0:\n", 381 | " d_ = dir_.pop(0)\n", 382 | " x_, y_ = udlr[d_](x,y)\n", 383 | " if is_valid((x_,y_), matrix, [0, 150]):\n", 384 | " x, y = visit(matrix, x_, y_, memory)\n", 385 | " break\n", 386 | " elif d_ == l:\n", 387 | " return x, y" 388 | ] 389 | }, 390 | { 391 | "cell_type": "code", 392 | "execution_count": 9, 393 | "metadata": { 394 | "ExecuteTime": { 395 | "end_time": "2020-07-28T19:57:33.661756Z", 396 | "start_time": "2020-07-28T19:57:33.644878Z" 397 | } 398 | }, 399 | "outputs": [], 400 | "source": [ 401 | "def bous_preprocess(area_map):\n", 402 | " \"\"\"\n", 403 | " Returns matrix that in the form\n", 404 | " that is read by the main algorithm.\n", 405 | " \"\"\"\n", 406 | " matrix = np.full(area_map.shape, 0, dtype=np.uint8)\n", 407 | " matrix[area_map == 0] = 255\n", 408 | " return matrix\n", 409 | "\n", 410 | "def do_everything(matrix, start_point, break_after_bous=False, timeit=True):\n", 411 | " t = lambda :timeit and time.time()\n", 412 | " times = {\n", 413 | " \"bous\": [],\n", 414 | " \"btrack\":[],\n", 415 | " \"astar\":[]\n", 416 | " }\n", 417 | " store_time = lambda name,value: timeit and times[name].append(value)\n", 418 | " start_time = t()\n", 419 | " \n", 420 | " width, height = matrix.shape\n", 421 | " radius=0.5\n", 422 | " diameter=int(2*radius)\n", 423 | "\n", 424 | " x, y = start_point\n", 425 | " memory=[]\n", 426 | " \n", 427 | " backtrack_counts = 0\n", 428 | " point_find_failed = 0 \n", 429 | "\n", 430 | " while True:\n", 431 | " sw = t()\n", 432 | " critical_x, critical_y = boustrophedon(matrix, diameter, x, y, memory)\n", 433 | " store_time(\"bous\", t() - sw)\n", 434 | " \n", 435 | " if break_after_bous:\n", 436 | " print(\"break point\",critical_x,critical_y)\n", 437 | " break\n", 438 | " \n", 439 | " sw = t()\n", 440 | " next_, is_end = backtracking_list(memory, diameter, matrix, critical_x, critical_y)\n", 441 | " x,y = next_\n", 442 | " store_time(\"btrack\", t() - sw)\n", 443 | " if is_end:\n", 444 | " break\n", 445 | " else:\n", 446 | " start = (critical_x,critical_y )\n", 447 | " end = (x, y)\n", 448 | " sw = t()\n", 449 | " path = AStarSearch(start, end, matrix, diameter, width, height)\n", 450 | " AstarPath(matrix, memory, path)\n", 451 | " store_time(\"astar\", t() - sw)\n", 452 | " \n", 453 | " times_avg = timeit and {k+\"_avg\":np.array(times[k]).mean() for k in times}\n", 454 | " times_tot = timeit and {k+\"_tot\":np.array(times[k]).sum() for k in times}\n", 455 | " times = timeit and {**times_avg, **times_tot}\n", 456 | " end_time = timeit and (time.time() - start_time)\n", 457 | " if timeit: times['total'] = end_time\n", 458 | " timeit and printer(times)\n", 459 | " return memory " 460 | ] 461 | }, 462 | { 463 | "cell_type": "code", 464 | "execution_count": 10, 465 | "metadata": { 466 | "ExecuteTime": { 467 | "end_time": "2020-07-28T19:57:34.664244Z", 468 | "start_time": "2020-07-28T19:57:34.558935Z" 469 | } 470 | }, 471 | "outputs": [], 472 | "source": [ 473 | "area_maps = get_all_area_maps(\"./test_maps/\")" 474 | ] 475 | }, 476 | { 477 | "cell_type": "code", 478 | "execution_count": 11, 479 | "metadata": { 480 | "ExecuteTime": { 481 | "end_time": "2020-07-28T19:57:35.785829Z", 482 | "start_time": "2020-07-28T19:57:35.530686Z" 483 | } 484 | }, 485 | "outputs": [ 486 | { 487 | "name": "stdout", 488 | "output_type": "stream", 489 | "text": [ 490 | "times ↓\n", 491 | " bous_avg : 0.0003654703180840675\n", 492 | " btrack_avg : 0.0012489785539343\n", 493 | " astar_avg : 0.0014533841091653576\n", 494 | " bous_tot : 0.017177104949951172\n", 495 | " btrack_tot : 0.05870199203491211\n", 496 | " astar_tot : 0.06685566902160645\n", 497 | " total : 0.1430509090423584\n", 498 | "\n", 499 | "metrics ↓\n", 500 | " points_to_visit : 772\n", 501 | " obstacle_points : 252\n", 502 | " points_visited : 772\n", 503 | " coverage_path_len : 1101\n", 504 | " coverage : 1.0\n", 505 | " redundancy : 0.42616580310880825\n", 506 | " area_shape : (32, 32)\n", 507 | "\n" 508 | ] 509 | }, 510 | { 511 | "data": { 512 | "image/png": "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\n", 513 | "text/plain": [ 514 | "
" 515 | ] 516 | }, 517 | "metadata": { 518 | "needs_background": "light" 519 | }, 520 | "output_type": "display_data" 521 | } 522 | ], 523 | "source": [ 524 | "area_map = area_maps[8]\n", 525 | "\n", 526 | "start_point = (12, 28)\n", 527 | "matrix = bous_preprocess(area_map)\n", 528 | "print(\"times ↓\")\n", 529 | "coverage_path = do_everything(matrix, start_point, break_after_bous=False)\n", 530 | "end_point = coverage_path[-1]\n", 531 | "\n", 532 | "print(\"\\nmetrics ↓\")\n", 533 | "printer(coverage_metrics(area_map, coverage_path))\n", 534 | "print()\n", 535 | "\n", 536 | "imshow(matrix, figsize=(8, 8), cmap=\"cividis\") #Shift + Tab\n", 537 | "imshow_scatter(coverage_path, alpha=0.4, color=\"black\",s=5)\n", 538 | "imshow_scatter([start_point],color=\"lightgreen\")\n", 539 | "imshow_scatter([end_point],color=\"red\")" 540 | ] 541 | }, 542 | { 543 | "cell_type": "code", 544 | "execution_count": 12, 545 | "metadata": { 546 | "ExecuteTime": { 547 | "end_time": "2020-07-28T19:57:38.318034Z", 548 | "start_time": "2020-07-28T19:57:38.307845Z" 549 | }, 550 | "scrolled": false 551 | }, 552 | "outputs": [], 553 | "source": [ 554 | "def cover_test(first_n_area_maps, times):\n", 555 | " t_ = []\n", 556 | " for j,area_map in tqdm(enumerate(area_maps[:first_n_area_maps]),total=first_n_area_maps):\n", 557 | " t = []\n", 558 | " for i in range(times):\n", 559 | " start_point = get_random_coords(area_map, 1)[0]\n", 560 | " matrix = bous_preprocess(area_map)\n", 561 | " try:\n", 562 | " s = time.time()\n", 563 | " coverage_path = do_everything(matrix, start_point, timeit=False)\n", 564 | " t.append(time.time() - s)\n", 565 | " except:\n", 566 | " print(f\"j: {j}, start: {start_point} FAILED\")\n", 567 | " imshow(area_map)\n", 568 | " return\n", 569 | " end_point = coverage_path[-1]\n", 570 | " metrics = coverage_metrics(area_map, coverage_path)\n", 571 | " if metrics[\"coverage\"] < 1.0:\n", 572 | " print(f\"j: {j}, start: {start_point}, end: {end_point}, cover:{metrics['coverage']} ,redun:{metrics['redundancy']}\")\n", 573 | " print()\n", 574 | " t_.append(np.array(t).mean())\n", 575 | " return t_" 576 | ] 577 | }, 578 | { 579 | "cell_type": "code", 580 | "execution_count": 13, 581 | "metadata": { 582 | "ExecuteTime": { 583 | "end_time": "2020-07-28T19:57:45.959707Z", 584 | "start_time": "2020-07-28T19:57:40.535366Z" 585 | } 586 | }, 587 | "outputs": [ 588 | { 589 | "data": { 590 | "application/vnd.jupyter.widget-view+json": { 591 | "model_id": "705f1cc9fb2f4a5980590d46c08e58b8", 592 | "version_major": 2, 593 | "version_minor": 0 594 | }, 595 | "text/plain": [ 596 | "HBox(children=(FloatProgress(value=0.0, max=11.0), HTML(value='')))" 597 | ] 598 | }, 599 | "metadata": {}, 600 | "output_type": "display_data" 601 | }, 602 | { 603 | "name": "stderr", 604 | "output_type": "stream", 605 | "text": [ 606 | "/Users/alan/opt/anaconda3/envs/data_sci/lib/python3.7/site-packages/ipykernel_launcher.py:22: UserWarning: number of iterations has exceeded max iterations\n" 607 | ] 608 | }, 609 | { 610 | "name": "stdout", 611 | "output_type": "stream", 612 | "text": [ 613 | "\n" 614 | ] 615 | }, 616 | { 617 | "data": { 618 | "text/plain": [ 619 | "[0.032391834259033206,\n", 620 | " 0.022753334045410155,\n", 621 | " 0.029361772537231445,\n", 622 | " 0.052572011947631836,\n", 623 | " 0.028550124168395995,\n", 624 | " 0.028410506248474122,\n", 625 | " 0.026017594337463378,\n", 626 | " 0.08865394592285156,\n", 627 | " 0.06439673900604248,\n", 628 | " 0.06884322166442872,\n", 629 | " 0.08887407779693604]" 630 | ] 631 | }, 632 | "execution_count": 13, 633 | "metadata": {}, 634 | "output_type": "execute_result" 635 | } 636 | ], 637 | "source": [ 638 | "# If returns time without any print then SUCCESS!\n", 639 | "cover_test(11,10) # cover test for all the maps <= 50, 50" 640 | ] 641 | }, 642 | { 643 | "cell_type": "code", 644 | "execution_count": 14, 645 | "metadata": { 646 | "ExecuteTime": { 647 | "end_time": "2020-07-28T19:58:05.001381Z", 648 | "start_time": "2020-07-28T19:57:48.159885Z" 649 | }, 650 | "scrolled": false 651 | }, 652 | "outputs": [ 653 | { 654 | "data": { 655 | "application/vnd.jupyter.widget-view+json": { 656 | "model_id": "a53f03e5d27b491ab2f53e0799efca5f", 657 | "version_major": 2, 658 | "version_minor": 0 659 | }, 660 | "text/plain": [ 661 | "HBox(children=(FloatProgress(value=0.0, max=21.0), HTML(value='')))" 662 | ] 663 | }, 664 | "metadata": {}, 665 | "output_type": "display_data" 666 | }, 667 | { 668 | "name": "stdout", 669 | "output_type": "stream", 670 | "text": [ 671 | "# 0 :: (32, 32) corners_0.png\n", 672 | " bous_avg : 0.0006066295835706922\n", 673 | " btrack_avg : 0.0012082788679334852\n", 674 | " astar_avg : 0.00030307208790498623\n", 675 | " bous_tot : 0.010919332504272461\n", 676 | " btrack_tot : 0.021749019622802734\n", 677 | " astar_tot : 0.005152225494384766\n", 678 | " total : 0.03796982765197754\n", 679 | " points_to_visit : 716\n", 680 | " obstacle_points : 308\n", 681 | " points_visited : 716\n", 682 | " coverage_path_len : 807\n", 683 | " coverage : 1.0\n", 684 | " redundancy : 0.12709497206703912\n", 685 | " area_shape : (32, 32)\n", 686 | "adjacency test pass : True\n", 687 | "\n", 688 | "# 2 :: (256, 256) comb_9.png\n", 689 | " bous_avg : 0.0012152791023254395\n", 690 | " btrack_avg : 0.014023769927281205\n", 691 | " astar_avg : 0.006197479535948555\n", 692 | " bous_tot : 0.4520838260650635\n", 693 | " btrack_tot : 5.216842412948608\n", 694 | " astar_tot : 2.299264907836914\n", 695 | " total : 7.97052001953125\n", 696 | " points_to_visit : 33385\n", 697 | " obstacle_points : 32151\n", 698 | " points_visited : 33385\n", 699 | " coverage_path_len : 38477\n", 700 | " coverage : 1.0\n", 701 | " redundancy : 0.15252358843792124\n", 702 | " area_shape : (256, 256)\n", 703 | "adjacency test pass : True\n", 704 | "\n", 705 | "# 3 :: (144, 256) comb_8.png\n", 706 | " bous_avg : 0.003343343734741211\n", 707 | " btrack_avg : 0.011790562211797478\n", 708 | " astar_avg : 0.008448570966720581\n", 709 | " bous_tot : 0.2975575923919678\n", 710 | " btrack_tot : 1.0493600368499756\n", 711 | " astar_tot : 0.7434742450714111\n", 712 | " total : 2.0910308361053467\n", 713 | " points_to_visit : 24720\n", 714 | " obstacle_points : 12144\n", 715 | " points_visited : 24720\n", 716 | " coverage_path_len : 26085\n", 717 | " coverage : 1.0\n", 718 | " redundancy : 0.05521844660194164\n", 719 | " area_shape : (144, 256)\n", 720 | "adjacency test pass : True\n", 721 | "\n", 722 | "# 4 :: (32, 32) pipes_0.png\n", 723 | " bous_avg : 0.0006602128346761067\n", 724 | " btrack_avg : 0.0009100437164306641\n", 725 | " astar_avg : 0.00024156911032540456\n", 726 | " bous_tot : 0.009903192520141602\n", 727 | " btrack_tot : 0.013650655746459961\n", 728 | " astar_tot : 0.003381967544555664\n", 729 | " total : 0.027039051055908203\n", 730 | " points_to_visit : 689\n", 731 | " obstacle_points : 335\n", 732 | " points_visited : 689\n", 733 | " coverage_path_len : 779\n", 734 | " coverage : 1.0\n", 735 | " redundancy : 0.13062409288824384\n", 736 | " area_shape : (32, 32)\n", 737 | "adjacency test pass : True\n", 738 | "\n", 739 | "# 5 :: (32, 32) pipes_1.png\n", 740 | " bous_avg : 0.0004187634116724918\n", 741 | " btrack_avg : 0.0008177380812795539\n", 742 | " astar_avg : 0.0001951720979478624\n", 743 | " bous_tot : 0.007956504821777344\n", 744 | " btrack_tot : 0.015537023544311523\n", 745 | " astar_tot : 0.0035130977630615234\n", 746 | " total : 0.027118206024169922\n", 747 | " points_to_visit : 639\n", 748 | " obstacle_points : 385\n", 749 | " points_visited : 639\n", 750 | " coverage_path_len : 764\n", 751 | " coverage : 1.0\n", 752 | " redundancy : 0.19561815336463217\n", 753 | " area_shape : (32, 32)\n", 754 | "adjacency test pass : True\n", 755 | "\n", 756 | "# 6 :: (32, 32) pipes_2.png\n", 757 | " bous_avg : 0.00033684730529785154\n", 758 | " btrack_avg : 0.0008809757232666015\n", 759 | " astar_avg : 0.001022746165593465\n", 760 | " bous_tot : 0.008421182632446289\n", 761 | " btrack_tot : 0.02202439308166504\n", 762 | " astar_tot : 0.024545907974243164\n", 763 | " total : 0.055140018463134766\n", 764 | " points_to_visit : 654\n", 765 | " obstacle_points : 370\n", 766 | " points_visited : 654\n", 767 | " coverage_path_len : 973\n", 768 | " coverage : 1.0\n", 769 | " redundancy : 0.48776758409785925\n", 770 | " area_shape : (32, 32)\n", 771 | "adjacency test pass : True\n", 772 | "\n", 773 | "# 7 :: (32, 32) caves_1.png\n", 774 | " bous_avg : 0.0013619661331176758\n", 775 | " btrack_avg : 0.0010527968406677246\n", 776 | " astar_avg : 0.002979346684047154\n", 777 | " bous_tot : 0.010895729064941406\n", 778 | " btrack_tot : 0.008422374725341797\n", 779 | " astar_tot : 0.020855426788330078\n", 780 | " total : 0.04027700424194336\n", 781 | " points_to_visit : 794\n", 782 | " obstacle_points : 230\n", 783 | " points_visited : 794\n", 784 | " coverage_path_len : 893\n", 785 | " coverage : 1.0\n", 786 | " redundancy : 0.12468513853904284\n", 787 | " area_shape : (32, 32)\n", 788 | "adjacency test pass : True\n", 789 | "\n", 790 | "# 8 :: (32, 32) caves_0.png\n", 791 | " bous_avg : 0.0005809980280259077\n", 792 | " btrack_avg : 0.0010146533741670497\n", 793 | " astar_avg : 0.0005776435136795044\n", 794 | " bous_tot : 0.00987696647644043\n", 795 | " btrack_tot : 0.017249107360839844\n", 796 | " astar_tot : 0.00924229621887207\n", 797 | " total : 0.03648209571838379\n", 798 | " points_to_visit : 725\n", 799 | " obstacle_points : 299\n", 800 | " points_visited : 725\n", 801 | " coverage_path_len : 896\n", 802 | " coverage : 1.0\n", 803 | " redundancy : 0.2358620689655173\n", 804 | " area_shape : (32, 32)\n", 805 | "adjacency test pass : True\n", 806 | "\n", 807 | "# 9 :: (32, 32) center_0.png\n", 808 | " bous_avg : 0.0007597666520338792\n", 809 | " btrack_avg : 0.0009075494912954477\n", 810 | " astar_avg : 0.00020899375279744467\n", 811 | " bous_tot : 0.00987696647644043\n", 812 | " btrack_tot : 0.01179814338684082\n", 813 | " astar_tot : 0.002507925033569336\n", 814 | " total : 0.0242922306060791\n", 815 | " points_to_visit : 723\n", 816 | " obstacle_points : 301\n", 817 | " points_visited : 723\n", 818 | " coverage_path_len : 798\n", 819 | " coverage : 1.0\n", 820 | " redundancy : 0.10373443983402497\n", 821 | " area_shape : (32, 32)\n", 822 | "adjacency test pass : True\n", 823 | "\n", 824 | "# 10 :: (144, 256) center_1.png\n" 825 | ] 826 | }, 827 | { 828 | "name": "stderr", 829 | "output_type": "stream", 830 | "text": [ 831 | "/Users/alan/opt/anaconda3/envs/data_sci/lib/python3.7/site-packages/ipykernel_launcher.py:22: UserWarning: number of iterations has exceeded max iterations\n" 832 | ] 833 | }, 834 | { 835 | "name": "stdout", 836 | "output_type": "stream", 837 | "text": [ 838 | " bous_avg : 0.028317672865731374\n", 839 | " btrack_avg : 0.01696244307926723\n", 840 | " astar_avg : 0.018626616551325872\n", 841 | " bous_tot : 0.39644742012023926\n", 842 | " btrack_tot : 0.2374742031097412\n", 843 | " astar_tot : 0.24214601516723633\n", 844 | " total : 0.8762199878692627\n", 845 | " points_to_visit : 36713\n", 846 | " obstacle_points : 151\n", 847 | " points_visited : 36713\n", 848 | " coverage_path_len : 37074\n", 849 | " coverage : 1.0\n", 850 | " redundancy : 0.009833029172227725\n", 851 | " area_shape : (144, 256)\n", 852 | "adjacency test pass : True\n", 853 | "\n", 854 | "# 11 :: (50, 144) comb_5.png\n", 855 | " bous_avg : 0.0028441373039694395\n", 856 | " btrack_avg : 0.0018088396857766545\n", 857 | " astar_avg : 0.001951858401298523\n", 858 | " bous_tot : 0.04835033416748047\n", 859 | " btrack_tot : 0.030750274658203125\n", 860 | " astar_tot : 0.031229734420776367\n", 861 | " total : 0.11046028137207031\n", 862 | " points_to_visit : 4383\n", 863 | " obstacle_points : 2817\n", 864 | " points_visited : 4383\n", 865 | " coverage_path_len : 4620\n", 866 | " coverage : 1.0\n", 867 | " redundancy : 0.054072553045859006\n", 868 | " area_shape : (50, 144)\n", 869 | "adjacency test pass : True\n", 870 | "\n", 871 | "# 12 :: (50, 144) comb_4.png\n", 872 | " bous_avg : 0.0014530294819882041\n", 873 | " btrack_avg : 0.0016398429870605469\n", 874 | " astar_avg : 0.0013732008031896643\n", 875 | " bous_tot : 0.05521512031555176\n", 876 | " btrack_tot : 0.06231403350830078\n", 877 | " astar_tot : 0.05080842971801758\n", 878 | " total : 0.16878604888916016\n", 879 | " points_to_visit : 4161\n", 880 | " obstacle_points : 3039\n", 881 | " points_visited : 4161\n", 882 | " coverage_path_len : 4597\n", 883 | " coverage : 1.0\n", 884 | " redundancy : 0.10478250420571977\n", 885 | " area_shape : (50, 144)\n", 886 | "adjacency test pass : True\n", 887 | "\n", 888 | "# 13 :: (50, 144) comb_6.png\n", 889 | " bous_avg : 0.004716277122497559\n", 890 | " btrack_avg : 0.0033800899982452393\n", 891 | " astar_avg : 0.008722909291585286\n", 892 | " bous_tot : 0.07546043395996094\n", 893 | " btrack_tot : 0.05408143997192383\n", 894 | " astar_tot : 0.1308436393737793\n", 895 | " total : 0.2605278491973877\n", 896 | " points_to_visit : 6042\n", 897 | " obstacle_points : 1158\n", 898 | " points_visited : 6042\n", 899 | " coverage_path_len : 6823\n", 900 | " coverage : 1.0\n", 901 | " redundancy : 0.12926183382985768\n", 902 | " area_shape : (50, 144)\n", 903 | "adjacency test pass : True\n", 904 | "\n", 905 | "# 14 :: (144, 256) comb_7.png\n", 906 | " bous_avg : 0.0014935346041324634\n", 907 | " btrack_avg : 0.011046377357077483\n", 908 | " astar_avg : 0.004979571092475966\n", 909 | " bous_tot : 0.3091616630554199\n", 910 | " btrack_tot : 2.286600112915039\n", 911 | " astar_tot : 1.0257916450500488\n", 912 | " total : 3.6228737831115723\n", 913 | " points_to_visit : 25866\n", 914 | " obstacle_points : 10998\n", 915 | " points_visited : 25866\n", 916 | " coverage_path_len : 28060\n", 917 | " coverage : 1.0\n", 918 | " redundancy : 0.08482177375705557\n", 919 | " area_shape : (144, 256)\n", 920 | "adjacency test pass : True\n", 921 | "\n", 922 | "# 15 :: (50, 144) comb_3.png\n", 923 | " bous_avg : 0.0005339840862238519\n", 924 | " btrack_avg : 0.002218738894596278\n", 925 | " astar_avg : 0.000689565010790555\n", 926 | " bous_tot : 0.05713629722595215\n", 927 | " btrack_tot : 0.23740506172180176\n", 928 | " astar_tot : 0.07309389114379883\n", 929 | " total : 0.3682999610900879\n", 930 | " points_to_visit : 3680\n", 931 | " obstacle_points : 3520\n", 932 | " points_visited : 3680\n", 933 | " coverage_path_len : 4602\n", 934 | " coverage : 1.0\n", 935 | " redundancy : 0.25054347826086953\n", 936 | " area_shape : (50, 144)\n", 937 | "adjacency test pass : True\n", 938 | "\n", 939 | "# 16 :: (32, 32) comb_11.png\n", 940 | " bous_avg : 0.00030965696681629527\n", 941 | " btrack_avg : 0.0010551918636668813\n", 942 | " astar_avg : 0.0005224305529927098\n", 943 | " bous_tot : 0.013624906539916992\n", 944 | " btrack_tot : 0.04642844200134277\n", 945 | " astar_tot : 0.022464513778686523\n", 946 | " total : 0.0827491283416748\n", 947 | " points_to_visit : 772\n", 948 | " obstacle_points : 252\n", 949 | " points_visited : 772\n", 950 | " coverage_path_len : 1114\n", 951 | " coverage : 1.0\n", 952 | " redundancy : 0.4430051813471503\n", 953 | " area_shape : (32, 32)\n", 954 | "adjacency test pass : True\n", 955 | "\n", 956 | "# 17 :: (50, 50) comb_2.png\n", 957 | " bous_avg : 0.00043979956179249045\n", 958 | " btrack_avg : 0.0013287845922976124\n", 959 | " astar_avg : 0.0007332464059193929\n", 960 | " bous_tot : 0.02155017852783203\n", 961 | " btrack_tot : 0.06511044502258301\n", 962 | " astar_tot : 0.03519582748413086\n", 963 | " total : 0.12210702896118164\n", 964 | " points_to_visit : 1318\n", 965 | " obstacle_points : 1182\n", 966 | " points_visited : 1318\n", 967 | " coverage_path_len : 1800\n", 968 | " coverage : 1.0\n", 969 | " redundancy : 0.3657056145675266\n", 970 | " area_shape : (50, 50)\n", 971 | "adjacency test pass : True\n", 972 | "\n", 973 | "# 18 :: (50, 50) comb_0.png\n", 974 | " bous_avg : 0.0007387399673461914\n", 975 | " btrack_avg : 0.0012377926281520299\n", 976 | " astar_avg : 0.0003067740687617549\n", 977 | " bous_tot : 0.02068471908569336\n", 978 | " btrack_tot : 0.034658193588256836\n", 979 | " astar_tot : 0.008282899856567383\n", 980 | " total : 0.06380581855773926\n", 981 | " points_to_visit : 1529\n", 982 | " obstacle_points : 971\n", 983 | " points_visited : 1529\n", 984 | " coverage_path_len : 1700\n", 985 | " coverage : 1.0\n", 986 | " redundancy : 0.11183780248528441\n", 987 | " area_shape : (50, 50)\n", 988 | "adjacency test pass : True\n", 989 | "\n", 990 | "# 19 :: (32, 32) comb_12.png\n", 991 | " bous_avg : 0.0001989910679478799\n", 992 | " btrack_avg : 0.0009345623754685925\n", 993 | " astar_avg : 0.00037555225559922513\n", 994 | " bous_tot : 0.012337446212768555\n", 995 | " btrack_tot : 0.057942867279052734\n", 996 | " astar_tot : 0.022908687591552734\n", 997 | " total : 0.09347295761108398\n", 998 | " points_to_visit : 610\n", 999 | " obstacle_points : 414\n", 1000 | " points_visited : 610\n", 1001 | " coverage_path_len : 1244\n", 1002 | " coverage : 1.0\n", 1003 | " redundancy : 1.039344262295082\n", 1004 | " area_shape : (32, 32)\n", 1005 | "adjacency test pass : True\n", 1006 | "\n", 1007 | "# 20 :: (50, 50) comb_1.png\n", 1008 | " bous_avg : 0.0006266902474796071\n", 1009 | " btrack_avg : 0.0012745156007654527\n", 1010 | " astar_avg : 0.0009198839014226741\n", 1011 | " bous_tot : 0.02130746841430664\n", 1012 | " btrack_tot : 0.04333353042602539\n", 1013 | " astar_tot : 0.030356168746948242\n", 1014 | " total : 0.09517312049865723\n", 1015 | " points_to_visit : 1472\n", 1016 | " obstacle_points : 1028\n", 1017 | " points_visited : 1472\n", 1018 | " coverage_path_len : 1881\n", 1019 | " coverage : 1.0\n", 1020 | " redundancy : 0.2778532608695652\n", 1021 | " area_shape : (50, 50)\n", 1022 | "adjacency test pass : True\n", 1023 | "\n", 1024 | "\n" 1025 | ] 1026 | } 1027 | ], 1028 | "source": [ 1029 | "display = False\n", 1030 | "for i,path in tqdm(enumerate(Path(\"./test_maps/\").iterdir()),total=21):\n", 1031 | " if path.suffix != \".png\":\n", 1032 | " continue\n", 1033 | " area_map = get_area_map(path)\n", 1034 | " # Remove conditional to run on all maps.\n", 1035 | "# if area_map.shape[1] > 50:\n", 1036 | "# continue\n", 1037 | " print(f\"# {i} :: \",area_map.shape, path.name)\n", 1038 | " start_point = get_random_coords(area_map, 1)[0]\n", 1039 | " matrix = bous_preprocess(area_map)\n", 1040 | " coverage_path = do_everything(matrix, start_point)\n", 1041 | " end_point = coverage_path[-1]\n", 1042 | " printer(coverage_metrics(area_map, coverage_path))\n", 1043 | " print(f\"adjacency test pass : {adjacency_test(coverage_path)}\")\n", 1044 | " print()\n", 1045 | "\n", 1046 | " if display:\n", 1047 | " imshow(matrix, cmap=\"Greys_r\",figsize=(10,10))\n", 1048 | " imshow_scatter(coverage_path, alpha=0.5, color=\"lightblue\",s=4)\n", 1049 | " imshow_scatter([start_point], color=\"black\")\n", 1050 | " imshow_scatter([end_point],color=\"red\")" 1051 | ] 1052 | } 1053 | ], 1054 | "metadata": { 1055 | "kernelspec": { 1056 | "display_name": "Python 3", 1057 | "language": "python", 1058 | "name": "python3" 1059 | }, 1060 | "language_info": { 1061 | "codemirror_mode": { 1062 | "name": "ipython", 1063 | "version": 3 1064 | }, 1065 | "file_extension": ".py", 1066 | "mimetype": "text/x-python", 1067 | "name": "python", 1068 | "nbconvert_exporter": "python", 1069 | "pygments_lexer": "ipython3", 1070 | "version": "3.7.6" 1071 | } 1072 | }, 1073 | "nbformat": 4, 1074 | "nbformat_minor": 4 1075 | } 1076 | -------------------------------------------------------------------------------- /test_shps/juhu.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_shps/juhu.zip -------------------------------------------------------------------------------- /test_shps/kamathipura.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_shps/kamathipura.zip -------------------------------------------------------------------------------- /test_shps/mumb.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_shps/mumb.zip -------------------------------------------------------------------------------- /test_shps/paris.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/18alantom/CoveragePathPlanning/3230b3cb859758f6e1c8cb8dae9e3e336f7a6423/test_shps/paris.zip --------------------------------------------------------------------------------