-
Notifications
You must be signed in to change notification settings - Fork 0
/
main_Astar_multigoal.py
75 lines (59 loc) · 2.34 KB
/
main_Astar_multigoal.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
import time
import matplotlib.pyplot as plt
from icecream import ic
from Astar_algorithm_multigoal import AStarAlgorithm
from draw_map_multigoal import RoutingMap_matplotlib
from create_riskzones import make_circle_riskzones, read_circle_riskzones
def main():
# map width (x), map height (y)
mapdimensions = (1000,1000)
# Astar algorithm stepsize
stepsize = 40
# locations in (x,y)
# root location enables multiple start locations
# root location is not shown in graph
root_location = (-30, 500)
start_locations = [(root_location[0] + stepsize, root_location[1] - 400),
(root_location[0] + stepsize, root_location[1]),
(root_location[0] + stepsize, root_location[1] + 400)]
#start_locations = [(10,100),(10,500),(10,900)]
#start_locations = [(100,100)]
goal_locations = [(800,700),(605,210),(305,408),(205,811),(503,905),(860,235)]
#goal_locations = [(800,700),(605,210)]
# number of obstacle
num_riskzones = 12
# (min,max) radius
riskzone_radius = (50,200)
riskzones = make_circle_riskzones(mapdimensions,
riskzone_radius,
num_riskzones)
#riskzones = read_circle_riskzones()
astar = AStarAlgorithm(start_locations,
goal_locations,
riskzones,
mapdimensions,
stepsize)
map = RoutingMap_matplotlib(start_locations,
goal_locations,
mapdimensions,
riskzones)
# draw map with start, goal and obstacles
map.draw_basemap()
print("map drawn")
start = time.perf_counter()
astar.astar_search()
astar.create_goalpaths()
astar.create_los_goalpaths_level3()
end = time.perf_counter()
map.draw_tree(astar.gridnodes)
map.draw_paths(astar.goalpaths)
map.draw_los_paths(astar.goalpaths)
# riskzone locations
print("zones: ",{circle.location:circle.radius for circle in riskzones})
print(f"tree drawn with {len(astar.gridnodes)} nodes")
print("calctime =", (end - start))
plt.savefig("figure.png")
plt.show()
# start main
if __name__ == '__main__':
main()