def test_route_planer(): map_10 = load_map('map-10.pickle') show_map(map_10) map_40 = load_map('map-40.pickle') show_map(map_40) planner = RoutePlanner(map_40) path = planner.find_shortest_path(start=5, goal=34) if path == [5, 16, 37, 12, 34]: print("great! Your code works for these inputs!") else: print("something is off, your code produced the following:") print(path)
# In[30]: # Run this cell first! from helpers import Map, load_map, show_map from student_code import shortest_path get_ipython().run_line_magic('load_ext', 'autoreload') get_ipython().run_line_magic('autoreload', '2') # ### Map Basics # In[31]: map_10 = load_map('map-10.pickle') show_map(map_10) # The map above (run the code cell if you don't see it) shows a disconnected network of 10 intersections. The two intersections on the left are connected to each other but they are not connected to the rest of the road network. On the graph above, the edge between 2 nodes(intersections) represents a literal straight road not just an abstract connection of 2 cities. # # These `Map` objects have two properties you will want to use to implement A\* search: `intersections` and `roads` # # **Intersections** # # The `intersections` are represented as a dictionary. # # In this example, there are 10 intersections, each identified by an x,y coordinate. The coordinates are listed below. You can hover over each dot in the map above to see the intersection number. # In[32]: map_10.intersections
# In[1]: # Run this cell first! from helpers import Map, load_map, show_map from student_code import shortest_path get_ipython().run_line_magic('load_ext', 'autoreload') get_ipython().run_line_magic('autoreload', '2') # ### Map Basics # In[2]: map_10 = load_map('map-10.pickle') show_map(map_10) # The map above (run the code cell if you don't see it) shows a disconnected network of 10 intersections. The two intersections on the left are connected to each other but they are not connected to the rest of the road network. On the graph above, the edge between 2 nodes(intersections) represents a literal straight road not just an abstract connection of 2 cities. # # These `Map` objects have two properties you will want to use to implement A\* search: `intersections` and `roads` # # **Intersections** # # The `intersections` are represented as a dictionary. # # In this example, there are 10 intersections, each identified by an x,y coordinate. The coordinates are listed below. You can hover over each dot in the map above to see the intersection number. # In[3]: map_10.intersections
# Run this cell first! from helpers import Map, load_map_10, load_map_40, show_map import math get_ipython().run_line_magic('load_ext', 'autoreload') get_ipython().run_line_magic('autoreload', '2') # ### Map Basics # In[2]: map_10 = load_map_10() show_map(map_10) # The map above (run the code cell if you don't see it) shows a disconnected network of 10 intersections. The two intersections on the left are connected to each other but they are not connected to the rest of the road network. This map is quite literal in its expression of distance and connectivity. On the graph above, the edge between 2 nodes(intersections) represents a literal straight road not just an abstract connection of 2 cities. # # These `Map` objects have two properties you will want to use to implement A\* search: `intersections` and `roads` # # **Intersections** # # The `intersections` are represented as a dictionary. # # In this example, there are 10 intersections, each identified by an x,y coordinate. The coordinates are listed below. You can hover over each dot in the map above to see the intersection number. # In[3]:
def shortest_path(M, start, goal): ''' Shortest Path: Implements A* Algorithm ************************************* Udacity Lecture: Minimise value: f = g + h g(path) = path cost h(path) = h(state) = estimated distance to goal (straight line distance) f(path) = total path cost minimising g, helps keep the path short minimising h, helps keep focused on goal result: search strategy that is the best possible. Finds the shortest length path while expanding minimum number of paths possible Definition of a Problem: Initial state -> s0 Actions (s) -> {a1, a2, a3...} Result (s,a) -> s' GoalTest (s) -> Bool (True|False) PathCost ( si > aj > si+1 > aj+1 ....) -> cost value (n) StepCost (s, a, s') -> n (cost of action) **************************************** Reference: A* Search Algorithm (Wikipedia): https://en.wikipedia.org/wiki/A*_search_algorithm Introduction to A*: https://www.redblobgames.com/pathfinding/a-star/introduction.html Pathfinding using A*: http://web.mit.edu/eranki/www/tutorials/search/ Finding max, min values of a multi-dimensional dictionary: https://stackoverflow.com/questions/20990282/find-max-min-of-values-in-multidimensional-dict Return key associated with min value of a dict if min key is not in set: https://stackoverflow.com/questions/40668338/return-key-associated-with-min-value-of-a-dict-if-min-key-is-not-in-set ***************************************** Parameters: M: Input Graph (Map) start: start node goal: goal node Returns: path: shortest path from start to goal (list) ''' print("Shortest path called") # Printing and debugging flags printing = True print("Print Mode: ", printing) debugging = False print("Debug Mode: ", debugging) # Initialise fronter, explored sets frontier = set([start]) explored = set([]) # Create dictionary to hold details of nodes on graph: # Previous: best previous node to get to the node (key) # gCost: path cost of reaching the node (key). Default value 1000 # hCost: heuristic (straight line distance to goal) of the node (key). Default value 1000 # totalCost: totalCost (f = g + h) of the node (key). Default value 10000 nodes = {} for key in M.intersections.keys(): nodes[key] = {} nodes[key]["previous"] = None nodes[key]["gCost"] = 1000 nodes[key]["hCost"] = 1000 nodes[key]["totalCost"] = 10000 # Determine x, y co-ordinates of start and goal nodes xS, yS = M.intersections[start][0], M.intersections[start][1] xG, yG = M.intersections[goal][0], M.intersections[goal][1] # Set details of start node nodes[start]["previous"] = start nodes[start]["gCost"] = 0 nodes[start]["hCost"] = distanceBetween(xS, yS, xG, yG) nodes[start]["totalCost"] = distanceBetween(xS, yS, xG, yG) # Debugging: List to track nodes that have been visited, used for debugging if debugging: currentList = [start] # Continue while the frontier still contains nodes while bool(frontier): # Select the node with minimum total cost that has not been explored # (Return key of minimum value of totalCost (value), where keys are contained (intersection) in frontier) current = min(set(nodes).intersection(frontier), key=lambda k: float(nodes[k]["totalCost"])) # Debugging: If current node has not been previously evaluated, add to CurrentList if debugging: if current not in currentList: currentList.append(current) # If the current node is the goal, search finished, return full path if current == goal: print("Found Goal node") path = reconstructPath(nodes, current, start) print(path) if printing: print("Path Distance: ", pathCost(M, path)) print("Straight Line Distance: ", distanceToGoal(M, start, goal)) show_map(M, start=start, goal=goal, path=path) # Debugging: returns all nodes evaluated if debugging: print("Nodes travelled: ", currentList) printNodes(nodes) return path # Remove current node from the frontier set, add to explored set frontier.remove(current) explored.add(current) # Evaluate each node (road) attached to current node for i in M.roads[current]: # Only evaluate unexplored paths if i not in explored: # Add node to frontier if not on if i not in frontier: frontier.add(i) # Calculate potential total cost of the step on path tempCost = nodes[current]["gCost"] + stepCost( M, current, i) + distanceToGoal(M, i, goal) # If potential total cost is less than current total cost for node (i.e. better path), update node details if tempCost < nodes[i]["totalCost"]: nodes[i]["previous"] = current nodes[i]["gCost"] = nodes[current]["gCost"] + stepCost( M, current, i) nodes[i]["hCost"] = distanceToGoal(M, i, goal) nodes[i][ "totalCost"] = nodes[i]["gCost"] + nodes[i]["hCost"] # Debugging: See each node being evaluated if debugging: print("Better path found") print("Node: ", i, nodes[i]) return
def shortest_path(M, start, goal): #print("shortest path called") ''' Input map M is a map object M.intersections gives dictionary of intersection no. with corresponding coordinates on map M.roads is a list of list of the nodes [node,node,...] connected to each node M.roads[index] Start node 'start' is the start node for the algorithm Goal node is the node for the GoalTest, i.e the destination. Return the lowest cost path ''' #Getting coordinates of Start and Goal goal_pos = M.intersections[goal] start_pos = M.intersections[start] #initialise frontier and explored sets frontier = set() frontier.add(start) explored = set() #calculate estimated distance from goal for each node using heuristics. h-score in f = g+h #using Euclidean distance here heur = {} for index in M.intersections: pos = M.intersections[index] heur[index] = euclid_dist(pos, goal_pos) #print('pos',index,'heur',heur[index]) #Traverse, store the current nodes Traversed = [] #store dictionaries for tracing back the paths from end cameFrom = {} #store score gScores = {start: 0} #first g score is 0 fScores = {start: heur[start]} #f(start) = heuristic(start) current_node = start #it=0 epoch = 0 while len(frontier) > 0: print("epoch:", epoch) epoch += 1 print("current node", current_node) print("frontier", frontier) print("camefrom", cameFrom) current_node = lowest_f(fScores, frontier, M, cameFrom, start) Traversed.append(current_node) explored.add(current_node) if goalTest(goal, explored): path = reconstruct_path(cameFrom, current_node) show_map(M, start, goal, path) return path frontier.remove(current_node) #add new items to frontier and add to Nodes for each in M.roads[current_node]: #neighbors of current node if each not in explored and each not in frontier: frontier.add(each) cost = euclid_dist(M.intersections[each], M.intersections[current_node]) #Nodes[each].append([each,cost,current_node]) #print(cameFrom) t_gScore = gScores[current_node] + cost if each in gScores: if t_gScore > gScores[each]: continue cameFrom[each] = current_node gScores[each] = t_gScore fScores[each] = gScores[each] + heur[each] #show_map(M,start,goal,Traversed) return None
def shortest_path(M, start, goal): #print("shortest path called") ''' Input map M is a map object M.intersections gives dictionary of intersection no. with corresponding coordinates on map M.roads is a list of list of the nodes [node,node,...] connected to each node M.roads[index] Start node 'start' is the start node for the algorithm Goal node is the node for the GoalTest, i.e the destination. Return the lowest cost path ''' #Getting coordinates of Start and Goal goal_pos = M.intersections[goal] start_pos = M.intersections[start] #initialise frontier and explored sets frontier = set() frontier.add(start) explored = set() #calculate estimated distance from goal for each node using heuristics. h-score in f = g+h #using Euclidean distance here heur = {} for index in M.intersections: pos = M.intersections[index] heur[index] = euclid_dist(pos, goal_pos) #print('pos',index,'heur',heur[index]) #print(heur) ''' #Nodes Nodes = defaultdict(list) Nodes[start].append([start,0,'null']) #[state,cost_between_parent&node,parent] for each in frontier: #initialise nodes for each frontier connected to start cost = euclid_dist(start_pos,M.intersections[each]) Nodes[each].append([each,cost,start]) ''' #currenttraverse, store the current nodes Traversed = [] cameFrom = {} #store score gScores = {start: 0} fScores = {start: heur[start]} current_node = start #it=0 while len(frontier) > 0: current_node = lowest_f(fScores, frontier) Traversed.append(current_node) explored.add(current_node) if goalTest(goal, explored): path = reconstruct_path(cameFrom, current_node, M, start, goal) show_map(M, start, goal, path) return path frontier.remove(current_node) #add new items to frontier and add to Nodes for each in M.roads[current_node]: #neighbors of current node if each not in explored and each not in frontier: frontier.add(each) cost = euclid_dist(M.intersections[each], M.intersections[current_node]) #Nodes[each].append([each,cost,current_node]) print("fscores:", fScores) print("gScores:", gScores) print("Camefrom:", cameFrom) print("Traversed:", Traversed) print(current_node) #print(cameFrom) t_gScore = gScores[current_node] + cost if each in gScores.keys(): if t_gScore > gScores[each]: continue cameFrom[each] = current_node gScores[each] = t_gScore fScores[each] = gScores[each] + heur[each] #show_map(M,start,goal,Traversed) return None
MAP_40_ANSWERS = [ (5, 34, [5, 16, 37, 12, 34]), (5, 5, [5]), (8, 24, [8, 14, 16, 37, 12, 17, 10, 24]) ] def test(shortest_path_function): correct = 0 for start, goal, answer_path in MAP_40_ANSWERS: path = shortest_path_function(map_40, start, goal) if path == answer_path: correct += 1 else: print("For start:", start, "Goal: ", goal, "Your path:", path, "Correct: ", answer_path) if correct == len(MAP_40_ANSWERS): print("All tests pass!") else: print("You passed", correct, "/", len(MAP_40_ANSWERS), "test cases") """Can uncomment this line test too""" #test(shortest_path) start, goal = 8, 24 path = shortest_path(map_40, start, goal) show_map(map_40, start, goal, path) print (path) print (time.clock() - start_time)
y2 = intersections[node_2][1] return math.sqrt((x1 - x2)**2 + (y1 - y2)**2) def get_tentative_gScore(self, current, neighbor): """Returns the tentative g Score of a node""" return self.get_gScore(current) + self.distance(current, neighbor) def heuristic_cost_estimate(self, node): """ Returns the heuristic cost estimate of a node """ return self.distance(node, self.goal) def calculate_fscore(self, node): """Calculate the f score of a node. """ return self.get_gScore(node) + self.heuristic_cost_estimate(node) def record_best_path_to(self, current, neighbor): """Record the best path to a node """ self.cameFrom[neighbor] = current self.gScore[neighbor] = self.get_tentative_gScore(current, neighbor) self.fScore[neighbor] = self.calculate_fscore(neighbor) ## Calculates shortest route from start to goal # Modifiy start and goal to see different results! start = 1 goal = 8 show_map(load_map_40(), start=start, goal=goal, path=PathPlanner(load_map_40(), start, goal).path)
# Run this cell first! from helpers import Map, load_map, show_map from student_code import shortest_path #%load_ext autoreload #%autoreload 2 map_10 = load_map('map-10.pickle') show_map(map_10)
def shortest_path(M,start,goal): print("shortest path called") cost_path = { 0 : ([start] , 0) } # { f_value(float : (path(list) , g_value till last node(float) ) } goal_cost_path = {} #{ f_value : path(list) while len(cost_path) > 0: min_cost_node = min(cost_path) c_cost_path =(min_cost_node , cost_path.pop(min_cost_node)) # (cost, (path, g_val)) c_path = (c_cost_path[1][0]) c_node = (c_cost_path[1][0])[-1] g_val = (c_cost_path[1][1]) o_g_val = g_val # check if goal is the start if c_node == goal: print('Already in goal') eff_path = [c_node] print('efficient_path: ', eff_path) show_map(M, start = start, goal = goal, path = eff_path) return eff_path break # Iterate over all available nodes for the current node for a_node in M.roads[c_node]: # check the node is not int previous node if a_node not in c_path: # Check if the node is the goal node if a_node == goal: up_a_path = c_path+ [a_node] up_g_val = o_g_val + get_distance(M.intersections[c_node] , M.intersections[a_node]) up_a_cost = up_g_val goal_cost_path[up_a_cost] = (up_a_path , up_g_val) # if the node is not the goal node if a_node != goal: up_a_path = c_path+ [a_node] up_g_val = o_g_val + get_distance(M.intersections[c_node] , M.intersections[a_node]) up_h_val = get_distance(M.intersections[a_node] , M.intersections[goal]) up_a_cost = up_g_val + up_h_val # check if there are any available paths to the goal if len(goal_cost_path) == 0: cost_path[up_a_cost] = (up_a_path , up_g_val) # If paths to the goal exists if len(goal_cost_path)>0: # Proceed only if the f_value is less than the f_value of the goal path if up_a_cost < o_g_val: cost_path[up_a_cost] = (up_a_path , up_g_val) # Efficient path is the one with minimum f_value eff_path = (goal_cost_path[min(goal_cost_path)][0]) print('efficient_path: ', eff_path) show_map(M, start = start, goal = goal, path = eff_path) return eff_path
# In[2]: # Run this cell first! from helpers import Map, load_map, show_map from student_code import shortest_path get_ipython().run_line_magic('load_ext', 'autoreload') get_ipython().run_line_magic('autoreload', '2') # ### Map Basics # In[8]: map_10 = load_map('map-10.pickle') show_map(map_10) # The map above (run the code cell if you don't see it) shows a disconnected network of 10 intersections. The two intersections on the left are connected to each other but they are not connected to the rest of the road network. On the graph above, the edge between 2 nodes(intersections) represents a literal straight road not just an abstract connection of 2 cities. # # These `Map` objects have two properties you will want to use to implement A\* search: `intersections` and `roads` # # **Intersections** # # The `intersections` are represented as a dictionary. # # In this example, there are 10 intersections, each identified by an x,y coordinate. The coordinates are listed below. You can hover over each dot in the map above to see the intersection number. # In[3]: map_10.intersections