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):
# 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)
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 []
