def get_path(self, goal):
current_node = self.current_node
if not current_node or not goal:
return []
elif current_node == goal:
return [goal]
if not self.last_target_node:
self.last_target_node = goal
if goal.unreachable:
log.info('Goal unreachable: %s', goal)
return [current_node]
elif self.last_target_node == goal:
if self.destination not in self.current_node.get_edges(self):
pass
else:
log.info('Reusing last path: %s', self.path)
return self.path
else:
self.last_target_node = goal
start = current_node
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current is goal:
break
# Get current node edges
for next in current.get_edges(self):
new_cost = cost_so_far[current] + current.cost(next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self._heuristic(goal, next)
frontier.put(next, priority)
came_from[next] = current
path = []
path.append(goal)
current = goal
while True:
try:
current = came_from[current]
except KeyError:
log.warning('No available path to %s', current)
current.unreachable = True
break
if not current:
break
path.append(current)
# Pop current node from path
path.pop()
return path
