def Astar(start_node, target_node, given_grid):
start_node = start_node
target_node = target_node
A_grid = given_grid
open_set = priorityQueue.PriorityQueue()
closed_set = []
current_node = None
is_better = False
path = []
open_set.push(start_node) # push start node into PQ
while not open_set.isEmpty(): # and not current_node == target_node:
current_node = open_set.pop() # take node with lowest f_cost
if current_node == target_node:
break
closed_set.append(current_node) # push this node to closed list as it's expanded
neighbors = current_node.get_neighbors(A_grid)
for key in neighbors:
if neighbors[key] not in closed_set:
is_better = neighbors[key].check_if_better(current_node, target_node, key)
if is_better:
open_set.push(neighbors[key])
last = current_node
print(last.x, last.y)
if open_set.isEmpty():
print("pusty")
while current_node.parent != current_node:
path.insert(0, current_node)
current_node = current_node.parent
return path
def astar(start_node, target_node, graph, x, y):
open_set = priorityQueue.PriorityQueue()
closed_set = set()
path = []
current_node = None
open_set.push(start_node) # push start node into PQ
while not open_set.is_empty():
current_node = open_set.pop() # take node with lowest f_cost
if current_node == graph[target_node.x][target_node.y]:
break
closed_set.add(
current_node) # push this node to closed list as it's expanded
neighbors = current_node.get_neighbors(graph, x, y)
for neighbor in neighbors:
if neighbor not in closed_set and not open_set.includes(
neighbor
): # Push node to open if is not in neither open or closed set
neighbor.calc_f_cost(current_node, target_node)
open_set.push(neighbor)
while current_node.parent != current_node:
path.insert(0, current_node)
current_node = current_node.parent
return path
def __init__(self,
myView,
gridWidth=5,
gridHeight=4,
hIsZero=True,
directNeighbors=False):
self.view = myView
self.width = gridWidth
self.height = gridHeight
self.directNeighbors = directNeighbors # false=8, true=4
self.vertexGrid = [[vertex.Vertex(x, y) for y in range(gridHeight)]
for x in range(gridWidth)]
print("Creating vertex grid with height:", gridHeight, "and width:",
gridWidth, "\n")
self.startCoordinates = [float('inf'), float('inf')]
self.goalCoordinates = [float('inf'), float('inf')]
self.obstacles = set()
self.startNode = None
self.goalNode = None
self.lastNode = None
self.hIsZero = hIsZero
self.priorityQueue = pq.PriorityQueue() #The priority queue U
self.planReady = False #True if a plan (= a path) is present
self.actualPath = [] #Sequence of vertices from start to goal
self.executer = None #Planexecuter
def setData(self, shape_tuple, dist, ind):
#self.data = data
(m, n) = shape_tuple
self.distance = dist
self.ind = ind
self.listOfObjects = []
self.queue = pq.PriorityQueue()
for i in range(0, m):
x = Obj.Object()
x.setIndex(i)
core = -1
neighbors = []
counter = 0
for dist in self.distance[i]:
if (dist > self.epsilon):
break
if (len(neighbors) <= self.minPts):
core = dist
neighbors.append((self.ind[counter], dist))
counter = counter + 1
if (core != -1):
if (len(neighbors) >= self.minPts):
x.setCoreDistance(core)
x.setNeighbors(neighbors)
self.listOfObjects.append(x)
def testPoints(key=lambda x: x):
pq = pqueue.PriorityQueue(key=key)
for i in range(10):
point = (random.randint(0, 10), random.randint(0, 10))
pq.push(point)
while not pq.isEmpty():
point = pq.pop()
print('Point: {0}\tDistance:{1}'.format(point,
point[0]**2 + point[1]**2))
def aStar(grid, start, goal):
"""
A* Algorithm implementation
"""
#Inititalize values
closedSet = []
openSet = priorityQueue.PriorityQueue()
openSet.put((start.row, start.column), 0)
cameFrom = {}
totExpandedCells = 0
g_score = {}
g_score[(start.row, start.column)] = 0
f_score = {}
f_score[(start.row, start.column)] = heuristic((start.row, start.column),
(goal.row, goal.column))
while not openSet.empty():
#Take the lowest f-value element from the openSet
(current_row, current_column) = openSet.get()
totExpandedCells = totExpandedCells + 1 #Required for analysis of algorithm
#If this is the goal state, return path
if current_row == goal.row and current_column == goal.column:
return reconstruct_path(cameFrom,
(goal.row, goal.column)), totExpandedCells
closedSet.append((current_row, current_column))
for neighbor in grid.neighbors(
grid.getCell(current_row, current_column)):
#Ignore cell if blocked
if grid.getCell(neighbor.row, neighbor.column).isBlocked():
continue
#Compute tentative g score
tentative_g_score = g_score[(current_row,
current_column)] + heuristic(
(neighbor.row, neighbor.column),
(current_row, current_column))
#if not the best path to this node -- Check closed List
if (neighbor.row, neighbor.column
) in closedSet and tentative_g_score > g_score.get(
(neighbor.row, neighbor.column), 0):
continue
#If the most promised path to the node -- Check openList
if tentative_g_score < g_score.get(
(neighbor.row, neighbor.column), 0) or (
neighbor.row,
neighbor.column) not in [i[1] for i in openSet.elements]:
#This is the best path till now. Record it!!!
cameFrom[(neighbor.row, neighbor.column)] = (current_row,
current_column)
g_score[(neighbor.row, neighbor.column)] = tentative_g_score
f_score[(neighbor.row, neighbor.column)] = g_score[
(neighbor.row, neighbor.column)] + heuristic(
(neighbor.row, neighbor.column),
(goal.row, goal.column))
#Tie breaking with bigger g
openSet.put((neighbor.row, neighbor.column),
10000 * f_score[(neighbor.row, neighbor.column)] -
(tentative_g_score))
#Tie breaking with smaller g
#openSet.put((neighbor.row,neighbor.column), 10000*f_score[(neighbor.row,neighbor.column)] + (tentative_g_score))
return None, totExpandedCells
def __init__(self): #, inicio, mapa):
self.estado_inicial = None #Estado(inicio,None)
self.fila_de_prioridade = fila.PriorityQueue()
self.objetivo = None #Estado(inicio,None)
self.mapa = None #mapa
def convert_list_to_pq(list):
pq = priorityQueue.PriorityQueue()
for item in list:
item.f_cost = item.manhattan_distance(target)
pq.push(item)
return pq
def pqueue(self):
return priorityQueue.PriorityQueue()
def aStar(grid, start, goal, h_new):
"""
A* Algorithm implementation with h_new for adaptive A*
"""
#Inititalize values
closedSet = []
openSet = priorityQueue.PriorityQueue()
openSet.put((start.row, start.column), 0)
cameFrom = {}
totExpandedCells = 0
#Required for tests
g_score = {}
g_score[(start.row, start.column)] = 0
f_score = {}
if (adaptive_heuristic((start.row, start.column), h_new)):
f_score[(start.row, start.column)] = adaptive_heuristic(
(start.row, start.column), h_new)
else:
f_score[(start.row, start.column)] = heuristic(
(start.row, start.column), (goal.row, goal.column))
while not openSet.empty():
#Take the lowest f-value element from the openSet
(current_row, current_column) = openSet.get()
totExpandedCells = totExpandedCells + 1 #Required for analysis of algorithm
#If this is the goal state, return path
if current_row == goal.row and current_column == goal.column:
path = reconstruct_path(cameFrom, (goal.row, goal.column))
gd_start = len(path) - 1
for i in closedSet:
h_new[i] = gd_start - g_score[i]
return path, totExpandedCells, h_new
closedSet.append((current_row, current_column))
for neighbor in grid.neighbors(
grid.getCell(current_row, current_column)):
#Ignore cell if blocked
if grid.getCell(neighbor.row, neighbor.column).isBlocked():
continue
#Compute tentative g score
tentative_g_score = g_score[(current_row,
current_column)] + heuristic(
(neighbor.row, neighbor.column),
(current_row, current_column))
#If not the best path to this node -- Check closed List
if (neighbor.row, neighbor.column
) in closedSet and tentative_g_score > g_score.get(
(neighbor.row, neighbor.column), 0):
continue
#If the most promised path to the node -- Check openList or undiscovered path
if tentative_g_score < g_score.get(
(neighbor.row, neighbor.column), 0) or (
neighbor.row,
neighbor.column) not in [i[1] for i in openSet.elements]:
#This is the best path till now. Record it!!!
cameFrom[(neighbor.row, neighbor.column)] = (current_row,
current_column)
g_score[(neighbor.row, neighbor.column)] = tentative_g_score
if (adaptive_heuristic((neighbor.row, neighbor.column),
h_new)):
f_score[(neighbor.row, neighbor.column)] = g_score[
(neighbor.row, neighbor.column)] + adaptive_heuristic(
(neighbor.row, neighbor.column), h_new)
else:
f_score[(neighbor.row, neighbor.column)] = g_score[
(neighbor.row, neighbor.column)] + heuristic(
(neighbor.row, neighbor.column),
(goal.row, goal.column))
openSet.put((neighbor.row, neighbor.column),
10000 * f_score[(neighbor.row, neighbor.column)] -
(tentative_g_score))
return None, totExpandedCells, h_new
def testIntegers():
pq = pqueue.PriorityQueue()
for i in range(10):
pq.push(i)
while not pq.isEmpty():
print(pq.pop())
import priorityQueue as pq
PQ = pq.PriorityQueue()
def printMenu():
print("Commands:")
print("\tEnter a to add\n\tEnter p to pop\n\tEnter d to display contents")
print("\tEnter t to top\n\tEnter Q to quit")
command = input("Please enter a command: ")
return command
def add():
number = int(input("Enter a number to add: "))
PQ.push(number)
print(str(number) + " added")
print("priority queue state is now:")
display()
def pop():
print(PQ)
item = PQ.pop()
print("Item removed was " + str(item))
display()
def display():
if PQ.size() == 0: