python深度优先广度优先和A star search

Posted 2021-02-09 林木声

 1 class Node:
 2     """
 3     This class describes a single node contained within a graph. 
 4     It has the following instannce level attributes:
 5     
 6     ID: An integer id for the node i.e. 1
 7     heuristic_cost: A float value representing the estimated 
 8                     cost to the goal node
 9     """    
10     def __init__(self, ID, heuristic_cost):
11         self.ID = ID
12         self.connected_nodes = []
13         self.heuristic_cost = heuristic_cost
14         
15     def __repr__(self):
16         ID = self.ID
17         hx = self.heuristic_cost
18         if len(self.connected_nodes)==0:
19             nodes = \'None\'
20         else:
21             nodes = \',\'.join(str(cn[1].ID) for cn in self.connected_nodes)
22         return \'Node:{}\\nh(n):{}\\nConnected Nodes:{}\'.format(ID, hx, nodes)
23         
24     def set_connected_nodes(self,connected_nodes):
25         """
26         Adds edges that lead from this node to other nodes:
27         
28         Parameters:
29         - connected_nodes: A list of tuples consisting of (cost, Node), 
30                            where \'cost\' is a floating point value 
31                            indicating the cost to get from this node 
32                            to \'Node\' and \'Node\' is a Node object
33         """
34         self.connected_nodes = connected_nodes
35     
36 def build_graph():
37     """
38     Builds the graph to be parsed by the search algorithms.
39     Returns: The starting node, which is the entry point into the graph
40     """
41     ids = range(13)
42     coords = [(0,0), (1,1), (1,0), (1,1), (5,2), (3,1), (3,0), 
43               (3,-1), (5,1), (4,1), (4,0), (4,-2), (7,0)]
44     
45     #https://en.wikipedia.org/wiki/Euclidean_distance
46     euclidean_distance = lambda x1y1, x2y2: ((x1y1[0]-x2y2[0])**2 +  (x1y1[1]-x2y2[1])**2)**(0.5)
47     
48     def build_connected_node_list(from_id, to_ids):
49         starting_coords = coords[from_id]
50         
51         connected_nodes = []
52         for to_id in to_ids:
53             connected_nodes.append((euclidean_distance(starting_coords, coords[to_id]), all_nodes[to_id]))
54             
55         return connected_nodes
56     
57     goal_coords = (7,0)
58     all_nodes = [Node(_id, euclidean_distance(coord, goal_coords)) for _id, coord in zip(ids, coords)]
59     
60     all_nodes[8].set_connected_nodes(build_connected_node_list(8, [12]))
61     all_nodes[10].set_connected_nodes(build_connected_node_list(10,[12]))
62     all_nodes[5].set_connected_nodes(build_connected_node_list(5, [8]))
63     all_nodes[6].set_connected_nodes(build_connected_node_list(6, [9, 10]))
64     all_nodes[7].set_connected_nodes(build_connected_node_list(7, [11]))
65     all_nodes[1].set_connected_nodes(build_connected_node_list(1, [4,5]))
66     all_nodes[2].set_connected_nodes(build_connected_node_list(2, [5,6]))
67     all_nodes[3].set_connected_nodes(build_connected_node_list(3, [7]))
68     all_nodes[0].set_connected_nodes(build_connected_node_list(0, [1,2,3]))
69     
70     return all_nodes[0]

1 # The starting node. You can use this cell to familiarize
2 # yourself with the node/graph structure
3 build_graph()

 

代码：

  1 import numpy
  2 
  3 def depth_first_search(starting_node, goal_node):
  4     """
  5     This function implements the depth first search algorithm
  6     
  7     Parameters:
  8     - starting_node: The entry node into the graph
  9     - goal_node: The integer ID of the goal node.
 10     
 11     Returns:
 12     A list containing the visited nodes in order they were visited with starting node
 13     always being the first node and the goal node always being the last
 14     """
 15     visited_nodes_in_order = []
 16     
 17     # YOUR CODE HERE
 18     #raise NotImplementedError()
 19     stack = []
 20     visited = set() # initialize explored set to empty
 21     stack.append(starting_node)
 22     
 23     while True:
 24         # if the stack is empty, then return failure
 25         if len(stack) == 0:
 26             return \'failure\'
 27         
 28         # choose a leaf node and remove it from the stack
 29         leafnode = stack.pop()
 30         
 31         if leafnode not in visited:
 32             visited_nodes_in_order.append(leafnode.ID)
 33         
 34         # if leaf node contain a good state, then return visited_nodes_in_order
 35         if leafnode.ID == goal_node:
 36             return visited_nodes_in_order
 37         
 38         # add the node to the explored set
 39         for cn in leafnode.connected_nodes:
 40             if cn[1] not in visited:
 41                 stack.append(leafnode)
 42                 stack.append(cn[1])
 43                 if cn[1] not in visited:
 44                     visited_nodes_in_order.append(cn[1].ID)
 45                     visited.add(cn[1])
 46                 break
 47 
 48 def iterative_deepening_depth_first_search(starting_node, goal_node):
 49     """
 50     This function implements the iterative deepening depth first search algorithm
 51     
 52     Parameters:
 53     - starting_node: The entry node into the graph
 54     - goal_node: The integer ID of the goal node.
 55     
 56     Returns:
 57     A list containing the visited node ids in order they were visited with starting node
 58     always being the first node and the goal node always being the last
 59     """
 60     visited_nodes_in_order = []
 61     
 62     # YOUR CODE HERE
 63     iterative_deepening_search(starting_node, goal_node, visited_nodes_in_order)
 64     
 65     return visited_nodes_in_order
 66 
 67 def iterative_deepening_search(starting_node, goal_node, visited_nodes_in_order):
 68     
 69     depth = 0
 70     while depth >= 0:
 71         result = depth_limited_search(starting_node, goal_node, depth, visited_nodes_in_order)
 72         
 73         if result != \'cutoff\' and result != \'failure\':
 74             return 
 75         
 76         depth = depth+1
 77     
 78 def depth_limited_search(starting_node, goal_node, limit, visited_nodes_in_order):
 79     return recursive_dls(starting_node, goal_node, limit, visited_nodes_in_order)
 80 
 81 def recursive_dls(node, goal_node, limit, visited_nodes_in_order):
 82     """
 83     :param node:
 84     :param goal_node:
 85     :param limit:
 86     :return: "failure"：fail,"cutoff"：cutoff,True：success
 87     """
 88     
 89     visited_nodes_in_order.append(node.ID)
 90     
 91     # goal test
 92     if node.ID == goal_node:
 93         return True
 94     elif limit == 0:
 95         return "cutoff"
 96     else:
 97         cutoff_occurred = False
 98         
 99         for cn in node.connected_nodes:
100             child = cn[1]
101             result = recursive_dls(child, goal_node, limit-1, visited_nodes_in_order)
102             
103             if result == "cutoff":
104                 cutoff_occurred = True
105             elif result != "failure":
106                 return True
107             
108         if cutoff_occurred:
109             return "cutoff"
110         else:
111             return "failure"
112 
113 def reconstruct_path(came_from, current):
114     path = [current.ID]
115     
116     while current in came_from:
117         current = came_from[current]
118         path.append(current.ID)
119     
120     return path
121 
122 
123 def a_star_search(starting_node, goal_node):
124     """
125     This function implements the A* search algorithm
126     
127     Parameters:
128     - starting_node: The entry node into the graph
129     - goal_node: The integer ID of the goal node.
130     
131     Returns:
132     A list containing the visited node ids in order they were visited with starting node
133     always being the first node and the goal node always being the last
134     """
135 
136     visited_nodes_in_order = []
137     
138     # YOUR CODE HERE
139     
140     # The set of nodes already evaluated
141     close_set = set()
142     
143     # The set of currently discovered nodes that are not evaluated yet.
144     # Initially, only the start node is known.
145     open_set = []
146     
147     # For each node, which node it can most efficiently be reached from.
148     # If a node can be reached from many nodes, cameFrom will eventually contain the
149     # most efficient previous step.
150     came_from = {}
151     
152     # For each node, the cost of getting from the start node to that node
153     gscore = {starting_node:0}
154     
155     # for each node, the total cost of getting from the start node to the goal
156     # by passing by that node. That value is partly known, partly heuristic.
157     fscore = {starting_node:starting_node.heuristic_cost}
158     
159     open_set.append((fscore[starting_node], starting_node))
160     
161     while open_set:
162         
163         # find the node in openSet having the lowest fScore[] value
164         lowscore = open_set[-1][0]
165         current = open_set[-1][1]
166         for item in open_set:
167             if item[0] < lowscore:
168                 current = item[1]
169                 lowscore = item[0]
170         
171         if current.ID == goal_node:
172             path = reconstruct_path(came_from, current)
173             for item in reversed(path):
174                 visited_nodes_in_order.append(item)
175                 
176         open_set.remove((lowscore, current))
177         
178         close_set.add(current)
179         
180         for cn in current.connected_nodes:
181             next = cn[1]
182             cost = cn[0]
183             
184             # Ignore the neighbor which is already evaluated
185             if next in close_set:
186                 continue
187             
188             # the cost from start to a neighbor via current
189             new_cost = gscore[current] + cost
190             
191             # Discover a new node
192             if next not in [i[1] for i in open_set]:
193                 open_set.append((fscore.get(next, numpy.inf), next))    
194             elif new_cost >= gscore.get(next, numpy.inf):
195                 continue
196             
197             # This path is the best until now. Record it
198             came_from[next] = current
199             gscore[next] = new_cost
200             fscore[next] = gscore[next] + next.heuristic_cost
201     
202     return visited_nodes_in_order

测试：

 1 goal_node = 12
 2 depth_first_search_answer = [0, 1, 4, 5, 8, 12]
 3 iterative_deepening_depth_first_search_answer = [0, 0, 1, 2, 3, 0, 1,
 4                                                  4, 5, 2, 5, 6, 3, 7,
 5                                                  0, 1, 4, 5, 8, 2, 5,
 6                                                  8, 6, 9, 10, 3, 7, 11,
 7                                                  0, 1, 4, 5, 8, 12]
 8 a_star_search_answer = [0, 2, 6, 10, 12]
 9 
10 assert (depth_first_search(build_graph(), goal_node)==depth_first_search_answer)
11 assert (iterative_deepening_depth_first_search(build_graph(), goal_node)==iterative_deepening_depth_first_search_answer)
12 assert (a_star_search(build_graph(), goal_node)==a_star_search_answer)