def PlanPath(self, env, alg, cost, heuristic, vis, alpha):
'''
This is our intelligent function that comes up with a path!
'''
collision_tolerance = 0.2
if self.origin == None or self.goal == None:
raise Exception("Path planner did not plan any path. Please specify the AUV's origin and goal first!")
# select path planning algorithm/method
if alg == "A*":
self.path = astar(auv = self, env=env, cost=cost, heuristic=heuristic, vis=vis, alpha = alpha)
elif alg == "Dijkstra":
self.path = global_planner(auv=self, env=env, cost=cost, vis=vis, alpha=alpha)
else:
speed_x = 0.3
speed_y = 0.20
speed_z = 0.00
vehicle_path = np.zeros((self.mission.discretization, 3))
for timepoint in range(self.mission.discretization):
x = self.origin[0] + speed_x * timepoint
y = self.origin[1] + speed_y * timepoint
z = self.origin[2] + speed_z * timepoint
# check whether coordinates are valid or whether they cause a crash
if env.collision_checker(x,y,z,collision_tolerance) == True:
sys.exit("Path planner failed. Your vehicle would have collided at X = " + str(x) + ", Y = " + str(y) + ", Z = " + str(z))
vehicle_path[timepoint] = np.array([x,y,z])
setattr(self,'path',vehicle_path)
def dist_matrix_get(self, block, dist_type):
dist_matrix = np.zeros([len(block), len(block)])
if dist_type == 'block':
for i in range(0, len(block) - 1):
for j in range(i + 1, len(block)):
dis = sum(abs(block[i] - block[j]))
dist_matrix[i, j] = dis
dist_matrix[j, i] = dis
elif dist_type == 'a*':
for i in range(0, len(block) - 1):
for j in range(i + 1, len(block)):
start = world2picture(block[i], self.origin,
self.resolution)
goal = world2picture(block[j], self.origin,
self.resolution)
dis = astar(self.map, (start[0], start[1]),
(goal[0], goal[1]))
dist_matrix[i, j] = dis
dist_matrix[j, i] = dis
elif dist_type == 'eu':
for i in range(0, len(block) - 1):
for j in range(i + 1, len(block)):
dis = np.linalg.norm(block[i] - block[j])
dist_matrix[i, j] = dis
dist_matrix[j, i] = dis
return dist_matrix
def georef(): #convertire date in matrice si executare
#creez matricea si pun obstacolele in ea
n = 41 #n trebuie sa fie impar
matx = np.zeros((n, n), dtype=int)
i = int((n - 1) / 2)
j = i
matx[i][j] = 8
for s in range(len(val[0])):
if val[1][s] != 0:
itemp = floor(sin(radians(val[0][s])) * val[1][s])
jtemp = floor(cos(radians(val[0][s])) * val[1][s])
#print(i+itemp,j+jtemp)
if abs(itemp) > i or abs(jtemp) > j:
continue
else:
matx[i + itemp][j + jtemp] = 1
#print(matx)
#go from->to
start = (i, j)
end = (i, 7)
maze = matx.tolist()
path = astar(maze, start,
end) #folosete algoritmul Astar pentru gasire drum
#print(path)
#pun 2 in matrice pe tot drumul
for i in path:
matx[i[0]][i[1]] = 2
#pun datele si intr-un fisier
f = open("data.txt", 'w')
np.savetxt(f, matx, fmt='%d')
f.close()
#ocolire
p(45)
for i in range(6, len(path)):
x = path[i - 2][0]
y = path[i - 1][0]
z = path[i][0]
#print(x,y,z,(x-y+y-z)/3.3,'\n')
srv((x - y + y - z) / 3.3)
if (x - y + y - z) != 0:
sleep(18.6 / 45)
sleep(13.2 / 45)
p(0)
srv(0)
sleep(0.1)
def constructBeelinePlan(self):
maze = np.ones((self.gridwidth, self.gridheight))
for i in range(self.gridwidth):
for j in range(self.gridheight):
loc = Coords(i, j)
if loc in self.safeLocations:
maze[self.gridheight - loc.y - 1][loc.x] = 0
start = (self.gridheight - self.agentState.location.y - 1,
self.agentState.location.x)
end = (self.gridheight - 1, 0)
print(maze)
path = astar(maze, start, end)
BPlan = []
for cells in path.plan():
BPlan.append(Coords(cells[1], self.gridheight - 1 - cells[0]))
print("Beeline Plan: ", BPlan)
return BPlan
def test():
right = 0
count = 0
d_better = 0
a_better = 0
for i in range(1, 200):
d = dijkstra(0, i)
a = astar(0, i)
print(d, '----', a)
if a == d:
right += 1
elif a > d:
d_better += 1
elif a < d:
a_better += 1
count += 1
accuracy = right / count
print(accuracy, '--', a_better, '--', d_better)
def main():
a = CheckGrid(sys.argv[1])
cn = ConStrN(a.alldict)
ConStrN.constraints = a.ConstraintsList
cn.InitilizeQueue()
#gui = App(CheckGrid.height,CheckGrid.width)
gui = App(CheckGrid.width, CheckGrid.height)
sn = SearchNode(cn,None)
def setRowColor(SN):
gui.clearGrid(CheckGrid.height,CheckGrid.width)
#gui.clearGrid(CheckGrid.height,CheckGrid.width)
for x in xrange(CheckGrid.width):
for y in xrange(CheckGrid.height):
if SN.Grid[x][y][0] ==1:
gui.drawrect(0,x,y,'red')
elif SN.Grid[x][y][0] != 1:
gui.drawrect(0,x,y,'grey')
SearchNode.onNodeupdate = setRowColor
def drawPath(path,generatednodes):
print "Lengde av stien: ",len(path)
print "Genrerte noder: ", generatednodes
ast = astar(sn)
ast.pathDoneUpdate = drawPath
print "Antall noder i treet: " ,(int(a.width) * int(a.height))
astar.popmethod = 'astar' # astar , bfs, dfs
p = ast.best_first_search(sn)
gui.mainloop()
def solveAStar(maze, start, finish):
path = astar(maze, start, finish)
path.insert(0, path[0])
crtDir = Directions.UP
for i in range(len(path) - 1, 0, -1):
maze[path[i][0]][path[i][1]] = PLAYER
printMaze(maze)
wm = createWorkingMatrix(maze, path[i], 4, 4, crtDir)
print("logic view:")
print(crtDir)
printWmatrix(wm, crtDir)
print("internal view:")
print(crtDir)
printMaze(wm)
time.sleep(1)
os.system("cls")
maze[path[i][0]][path[i][1]] = EMPTY
crtDir = nextDir(path[i], path[i - 1])
'8': (1, 3),
'9': (2, 0),
'10': (2, 1),
'11': (2, 2),
'12': (2, 3),
'13': (3, 0),
'14': (3, 1),
'15': (3, 2),
' ': (3, 3)
}
regular_a_star_time = weighted_astar_time = anytime_weighted_astar_time = 0
hueristic_weight = 1.4,
timer_start = time.time()
astar(start, goal)
timer_stop = time.time()
regular_a_star_time = timer_stop - timer_start
print('Regular A* execution time: ', regular_a_star_time)
'''
timer_start = time.time()
weighted_astar(start, goal)
timer_stop = time.time()
weighted_astar_time = timer_stop-timer_start
print('Weighted A* execution time: ', weighted_astar_time)
'''
timer_start = time.time()
anytime_weighted_astar(start, goal, hueristic_weight)
timer_stop = time.time()
anytime_weighted_astar_time = timer_stop - timer_start
print('Anytime Weighted A* execution time: ', anytime_weighted_astar_time)
user_input = input()
map = generate_manual_map()
start = [(7, 0), (10, 0), (44, 1), (26, 3), (5, 6), (25, 0)]
hueristic_weight = 1.1
start = (25, 0)
goal = (17, 47)
count = 0
regular_a_star_time = 0
anytime_weighted_astar_time = 0
print('Running 10000 iterations of A-star and Anytime-star')
while (count < 1):
timer_start = time.time()
astar_output = astar(start, goal, map)
timer_stop = time.time()
visualize_predefined_map_and_path(map, astar_output)
regular_a_star_time += timer_stop - timer_start
timer_start = time.time()
(anytime_wastar_output,
error) = anytime_weighted_astar(start, goal, map, hueristic_weight)
timer_stop = time.time()
visualize_predefined_map_and_path(map, anytime_wastar_output)
anytime_weighted_astar_time += timer_stop - timer_start
if (count % 100 == 0 and count != 0):
print(str(count) + ' iterations finished')
count = count + 1
print('\nTime Required for 10000 iterations')
print('Regular A-star: ', regular_a_star_time)
def update(self):
path = []
c = 1
move = None
currentTarget = None
while not self.gameover:
pygame.time.delay(DELAY)
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.gameover = True
# click event and get the position
if event.type ==pygame.MOUSEBUTTONDOWN and move is None:
mx,my=pygame.mouse.get_pos()
mx=int(mx/32)*32
my=int(my/32)*32
mouseRect = pygame.Rect(mx,my,FRAME_SIZE,FRAME_SIZE)
scenarioRects = self.getScenarioRects()
collideIndex = mouseRect.collidelist(scenarioRects)
x,y = self.getCoordsFromScenarioRectsIndex(collideIndex)
print(f'target: {x,y}')
gridType,_ = self.scenario[x][y]
print(f'gridType: {gridType}')
if gridType is GridItemType.ROAD:
_,r = self.scenario[x][y]
self.scenario[x][y] = (GridItemType.TARGET,r)
if currentTarget is not None:
a,b = currentTarget
_,d = self.scenario[a][b]
self.scenario[a][b] = (1,d)
currentTarget = (x,y)
scenario = self.getValueMatrix()
start = (self.player.y // FRAME_SIZE,self.player.x // FRAME_SIZE)
p,evaluated = astar(scenario,start,(y,x),Node.Manhattan)
npath = len(p)
for i in range(npath - 1):
y1,x1 = p[i]
y2,x2 = p[i+1]
difx = x2 - x1
dify = y2 - y1
if difx > 0:
path.append(Direction.RIGHT)
elif difx < 0:
path.append(Direction.LEFT)
elif dify > 0:
path.append(Direction.DOWN)
elif dify < 0:
path.append(Direction.UP)
for step in evaluated:
i,j = step
scenario[i][j] = 11
for step in p:
i,j = step
scenario[i][j] = 10
printPath(scenario)
#keys = pygame.key.get_pressed()
self.screen.fill(SCREEN_BACKGROUND_COLOR)
self.printScenario(DEBUG)
self.player.blit_on(self.screen,DEBUG)
if c == 9:
if len(path) > 0:
move = path.pop(0)
else:
move = None
c = 1
if move is not None:
self.player.direction = move
self.player.move()
c += 1
pygame.display.update()