def compute_path(self, start, goal):
# Implementation of the A* algorithm.
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from the open set
curr_node = open_set.pop_smallest()
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def compute_path_until_PM(self, start, goal, movType = 0, PM = 7):
# ======= Algoritmo A* =======
closed_set = {}
# Nodo inicial
start_node = self._Node(start.pos, start.face)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Extrae el nodo con mínimo f_score de open_set
curr_node = open_set.pop_smallest()
#~ print curr_node
# Comprobamos si hemos alcanzado goal y orientamos el valor de face
#~ print " Comp ",curr_node.coord," Con ",goal.pos
if curr_node.coord == goal.pos:
#~ print "Hemos alcanzado GOAL"
if curr_node.face != goal.face:
# Corregimos el valor de curr_node.face
new = self._Node(curr_node.coord, goal.face, curr_node.g_cost+abs(curr_node.face - goal.face) , 0, curr_node)
# PATH ENCONTRADO
return self._reconstruct_path_until_PM(new, PM)
else:
# PATH ENCONTRADO
return self._reconstruct_path_until_PM(curr_node, PM)
# Si no es goal, obtenemos los sucesores del nodo.
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord,movType, PM):
succ_node = self._Node(succ_coord)
# A cada nodo sucesor le calculamos el g_cost y el f_cost
(succ_node.g_cost, succ_node.face) = self._compute_g_cost(curr_node, succ_node, movType)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
#~ if succ_node.g_cost <= 10:
#~ print "Sucesor: ",succ_coord,"\nCoste G = ",succ_node.g_cost,"\nCoste F = ",succ_node.f_cost
continue
# Añadimos el nodo sucesor a open_set
if open_set.add(succ_node):
# Establecemos su predecesor
#~ if succ_node.g_cost <= 10:
#~ print "Sucesor: ",succ_coord,"\nCoste G = ",succ_node.g_cost,"\nCoste F = ",succ_node.f_cost
succ_node.pred = curr_node
# PATH NO ENCONTRADO
#~ print "Cjto: ",closed_set.keys()
#~ print "Path no encontrado"
#~ return ([], False, 0)
return self._reconstruct_path_until_PM(curr_node, 1)
def compute_path(self, start, goal):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
goal_node = self._Node(goal)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal_node)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
if curr_node.coord == goal_node.coord:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
#original only considers this node and the next node
#succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
# we want to consider where we are coming from as well, so we
# provide the parent node
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node,
curr_node.pred, start_node, goal_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal_node)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def compute_path(self, start, goal):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
if len(open_set) > 100:
t2 = time.clock()
timeWasted = t2 - self.t1
raise ValueError('path too long, wasted %f sec' % timeWasted)
curr_node = open_set.pop_smallest()
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def compute_path_until_PM(self, start, goal, movType = 0, PM = 7):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start.pos, start.face)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
# If we reached the tarjet Take care of END FACE
if curr_node.coord == goal.pos:
if curr_node.face != goal.face:
new = self._Node(curr_node.coord, goal.face, curr_node.g_cost+abs(curr_node.face - goal.face) , 0, curr_node)
return self._reconstruct_path_until_PM(new, PM)
else:
return self._reconstruct_path_until_PM(curr_node, PM)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord,movType, PM):
succ_node = self._Node(succ_coord)
(succ_node.g_cost, succ_node.face) = self._compute_g_cost(curr_node, succ_node, movType)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return [], False, 0
def compute_path(self, start, goal):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
#logging.debug("Checking successors for node: {0}".format(curr_node))
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def compute_path(self, start, goal):
# Implementation of the A* algorithm.
closed_set = {}
start_node = self._Node(start)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from the open set
curr_node = open_set.pop_smallest()
if curr_node.coord == goal:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def compute_path(self, start, goal):
""" Compute the path between the 'start' point and the
'goal' point.
The path is returned as an iterator to the points,
including the start and goal points themselves.
If no path was found, an empty list is returned.
"""
#
# Implementation of the A* algorithm.
#
closed_set = {}
start_node = self._Node(start)
goal_node = self._Node(goal)
start_node.g_cost = 0
start_node.f_cost = self._compute_f_cost(start_node, goal_node)
open_set = PriorityQueueSet()
open_set.add(start_node)
while len(open_set) > 0:
# Remove and get the node with the lowest f_score from
# the open set
#
curr_node = open_set.pop_smallest()
if curr_node.coord == goal_node.coord:
return self._reconstruct_path(curr_node)
closed_set[curr_node] = curr_node
for succ_coord in self.successors(curr_node.coord):
succ_node = self._Node(succ_coord)
#original only considers this node and the next node
#succ_node.g_cost = self._compute_g_cost(curr_node, succ_node)
# we want to consider where we are coming from as well, so we
# provide the parent node
succ_node.g_cost = self._compute_g_cost(curr_node, succ_node,
curr_node.pred, start_node, goal_node)
succ_node.f_cost = self._compute_f_cost(succ_node, goal_node)
if succ_node in closed_set:
continue
if open_set.add(succ_node):
succ_node.pred = curr_node
return []
def best_path(self, posA, posB):
results = []
closedList = []
openList = PriorityQueueSet()
startNode = Node(posA)
endNode = Node(posB)
# if start node or end node is impassable then this can not
# happen
if self.impassablePred(posA) or self.impassablePred(posB):
return []
startNode.costFromStart = self.costFunc(posA)
startNode.totalCost = startNode.costFromStart
reachedEndNode = False
pathEndNode = None
# add start node to the open list
openList.add(startNode, (startNode.totalCost, startNode.costFromStart))
while len(openList) > 0:
# logging.debug('------------------------------')
# logging.debug('open: %s' % str(openList))
# logging.debug('closed: %s' % str(closedList))
node = openList.pop()
# check if rNode is the end point, if so then we are done
if node == endNode:
reachedEndNode = True
pathEndNode = node
break
closedList.append(node)
# logging.debug('node: %s' % str(node))
adjacencies = self.adjacenciesFunc(node.elem)
for r in adjacencies:
# first check if impassable, if so then skip
if self.impassablePred(r):
continue
h = self.heuristicCostFunc(r, posB)
g = node.costFromStart + self.costFunc(r)
f = g + h
rNode = Node(r, parent=node, totalCost=f, costFromStart=g)
# check if rNode is already in the closed list and if
# so then ignore it
if rNode in closedList:
continue
# check if rNode is already in the open list and if so
# then if it's G cost is better or the same then skip
# to next node
if rNode in openList:
olNodeCost = openList.get(rNode)[1]
if olNodeCost <= rNode.costFromStart:
continue
# add rNode to the open list
openList.add(rNode, (rNode.totalCost, rNode.costFromStart))
# backtrack and copy results into the results buffer
if reachedEndNode:
n = pathEndNode
while n:
results.append(n.elem)
n = n.parent
# return it in the forward order
results.reverse()
return results
def best_path(self, posA, posB):
results = []
closedList = []
openList = PriorityQueueSet()
startNode = Node(posA)
endNode = Node(posB)
# if start node or end node is impassable then this can not
# happen
if self.impassablePred(posA) or self.impassablePred(posB):
return []
startNode.costFromStart = self.costFunc(posA)
startNode.totalCost = startNode.costFromStart
reachedEndNode = False
pathEndNode = None
# add start node to the open list
openList.add(startNode, (startNode.totalCost, startNode.costFromStart))
while len(openList) > 0:
#logging.debug('------------------------------')
#logging.debug('open: %s' % str(openList))
#logging.debug('closed: %s' % str(closedList))
node = openList.pop()
# check if rNode is the end point, if so then we are done
if node == endNode:
reachedEndNode = True
pathEndNode = node
break
closedList.append(node)
#logging.debug('node: %s' % str(node))
adjacencies = self.adjacenciesFunc(node.elem)
for r in adjacencies:
# first check if impassable, if so then skip
if self.impassablePred(r):
continue
h = self.heuristicCostFunc(r, posB)
g = node.costFromStart + self.costFunc(r)
f = g + h
rNode = Node(r, parent=node, totalCost=f, costFromStart=g)
# check if rNode is already in the closed list and if
# so then ignore it
if rNode in closedList:
continue
# check if rNode is already in the open list and if so
# then if it's G cost is better or the same then skip
# to next node
if rNode in openList:
olNodeCost = openList.get(rNode)[1]
if olNodeCost <= rNode.costFromStart:
continue
# add rNode to the open list
openList.add(rNode, (rNode.totalCost, rNode.costFromStart))
# backtrack and copy results into the results buffer
if reachedEndNode:
n = pathEndNode
while n:
results.append(n.elem)
n = n.parent
# return it in the forward order
results.reverse()
return results
