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
# 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
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
