def a_star(graph, start, goal, heuristic=euclidean_dist_heuristic): """ Args: graph (ExplorableGraph): Undirected graph to search. start (str): Key for the start node. goal (str): Key for the end node. heuristic: Function to determine distance heuristic. Default: euclidean_dist_heuristic. Returns: The best path as a list from the start and goal nodes (including both). """ if start == goal: return [] queue = PriorityQueue() queue.append((0, [0, start])) isVisited = sets.Set() while queue.size() > 0: dist, nodes = queue.pop() if nodes[-1] not in isVisited: isVisited.add(nodes[-1]) if nodes[-1] == goal: return nodes[1:] for node in graph[nodes[-1]]: newPath = copy.deepcopy(nodes) newPath.append(node) newPath[0] += graph[nodes[-1]][node]['weight'] newDist = newPath[0]+heuristic(graph, node, goal) queue.append((newDist, newPath)) raise Error('No path from {} to {}').format(start, goal)
def test_append_and_pop(self): """Test the append and pop functions""" queue = PriorityQueue() temp_list = [] for _ in xrange(10): a = random.randint(0, 10000) queue.append((a, 'a')) temp_list.append(a) temp_list = sorted(temp_list) for item in temp_list: popped = queue.pop() self.assertEqual(item, popped[0])
def run_algorithm(self): """ run the algorithm :return: True is there is a path from strat to end, False otherwise """ open_list = PriorityQueue(f=self.f) open_list.append(self.start_point) closed_list = set() while not len(open_list) == 0: next_n = open_list.pop() closed_list.add(next_n) if self.end_point == next_n: self.value = next_n.total_value() self.path = next_n.arrived_from return True else: self.path_count += 1 for suc in next_n.successors: suc.arrived_from = next_n.arrived_from + [self.get_direction(next_n.x, next_n.y, suc.x, suc.y)] suc.fathers = next_n.fathers + [next_n] if suc not in closed_list and suc not in open_list: open_list.append(copy.deepcopy(suc)) elif suc in open_list and self.f(suc) < open_list[suc]: del open_list[suc] open_list.append(copy.deepcopy(suc)) self.path = self.NO_PATH return False
def uniform_cost_search(problem): root = Node(problem.initial) frontier = PriorityQueue() frontier.append(root) explored = set() while True: if len(frontier) == 0: return "fail" node = frontier.pop() if problem.goal_test(node.state): return node explored.add(node) for action in problem.actions(node.state): child = node.child_node(problem, action) if child.state not in explored: print(child.state + " > ") frontier.append(child) elif child in frontier: item = frontier.__getitem__(child) if child.path_cost < item.path_cost: pass
def bidirectional_a_star(graph, start, goal, heuristic=euclidean_dist_heuristic): """ Args: graph (ExplorableGraph): Undirected graph to search. start (str): Key for the start node. goal (str): Key for the end node. heuristic: Function to determine distance heuristic. Default: euclidean_dist_heuristic. Returns: The best path as a list from the start and goal nodes (including both). """ if start == goal: return [] queue_s = PriorityQueue() queue_s.append((0, 0, start, '')) is_visited_s = {} queue_g = PriorityQueue() queue_g.append((0, 0, goal, '')) is_visited_g = {} small = [float('inf'),''] p = lambda g, s, t, n : (heuristic(g, n, t)-heuristic(g, n, s)+heuristic(g, t, s))/2 while queue_s.size()+queue_g.size() > 0: est_s, dist_s, node_s, pa_s = queue_s.pop() est_g, dist_g, node_g, pa_g = queue_g.pop() # not really sure about the ending criteria for a* if est_s+est_g >= small[0]+2*p(graph, start, goal, start): node = small[1] ret = [node] ptr = node while is_visited_s[ptr][1] != '': ret.append(is_visited_s[ptr][1]) ptr = is_visited_s[ptr][1] ret.reverse() ptr = node while is_visited_g[ptr][1] != '': ret.append(is_visited_g[ptr][1]) ptr = is_visited_g[ptr][1] return ret """ forward/backward search """ for queue, is_visited, is_visited_other, node, dist, pa, t, s in \ [[queue_s, is_visited_s, is_visited_g, node_s, dist_s, pa_s, goal, start], \ [queue_g, is_visited_g, is_visited_s, node_g, dist_g, pa_g, start, goal]]: if is_visited.get(node) is not None: continue is_visited[node] = [dist, pa] if is_visited_other.get(node) is not None: dist_cache, _ = is_visited_other[node] if dist_cache+dist < small[0]: small = [dist_cache+dist, node] else: for dest in graph[node]: new_dist = dist+graph[node][dest]['weight'] new_heuristic = p(graph,s,t,dest) queue.append((new_dist+new_heuristic, new_dist, dest, node)) raise Error('No path from {} to {}').format(start, goal)