def greedy(graph):
n = graph.get_total_vertex_count()
s = graph.s
t = graph.t
cost = graph.cost
h_list = heuristic(graph.point_list, graph.point_list[t])
path = [s]
p_remain = list(range(1, n))
while path[-1] != t:
u = path[-1]
next_vertices = []
check = False
for v in range(n):
if cost[u][v] < 1e9 and v in p_remain:
next_vertices.append(h_list[v])
check = True
else:
next_vertices.append(1e9)
if not check:
return False, []
u = min(enumerate(next_vertices), key=itemgetter(1))[0]
p_remain.remove(u)
path.append(u)
return True, path
def __init__(self, map: OccupancyGridMap, s_start: (int, int, int),
s_goal: (int, int, int)):
"""
:param map: the ground truth map of the environment provided by gui
:param s_start: start location
:param s_goal: end location
"""
## we need to also update our occupancy grid map
self.new_edges_and_old_costs = None
# algorithm start
self.s_start = s_start # now takes in an x, y and z
self.s_goal = s_goal # now takes in an x, y and z
self.s_last = s_start # now takes in an x, y and z
self.k_m = 0 # accumulation
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim, map.z_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
z_dim=map.z_dim,
exploration_setting='26N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int)):
"""
:param s: the vertex we want to calculate key
:return: Priority class of the two keys
"""
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start, s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)
def move_and_replan(self, robot_position: (int, int, int)):
path = [robot_position]
self.s_start = robot_position
self.s_last = self.s_start
self.compute_shortest_path()
while self.s_start != self.s_goal:
assert (self.rhs[self.s_start] !=
float('inf')), "There is no known path!"
succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False)
min_s = float('inf')
arg_min = None
for s_ in succ:
temp = self.c(self.s_start, s_) + self.g[s_]
if temp < min_s:
min_s = temp
arg_min = s_
### algorithm sometimes gets stuck here for some reason !!! FIX
self.s_start = arg_min
path.append(self.s_start)
# scan graph for changed costs
changed_edges_with_old_cost = self.rescan()
#print("len path: {}".format(len(path)))
# if any edge costs changed
if changed_edges_with_old_cost:
self.k_m += heuristic(self.s_last, self.s_start)
self.s_last = self.s_start
# for all directed edges (u,v) with changed edge costs
vertices = changed_edges_with_old_cost.vertices
for vertex in vertices:
v = vertex.pos
succ_v = vertex.edges_and_c_old
for u, c_old in succ_v.items():
c_new = self.c(u, v)
if c_old > c_new:
if u != self.s_goal:
self.rhs[u] = min(self.rhs[u],
self.c(u, v) + self.g[v])
elif self.rhs[u] == c_old + self.g[v]:
if u != self.s_goal:
min_s = float('inf')
succ_u = self.sensed_map.succ(vertex=u)
for s_ in succ_u:
temp = self.c(u, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[u] = min_s
self.update_vertex(u)
self.compute_shortest_path()
if len(path) > 1:
return path, self.g, self.rhs
return None, None, None
def c(self, u: (int, int), v: (int, int)) -> float:
"""
calcuclate the cost between nodes
:param u: from vertex
:param v: to vertex
:return: euclidean distance to traverse. inf if obstacle in path
"""
if not self.sensed_map.is_unoccupied(u) or not self.sensed_map.is_unoccupied(v):
return float('inf')
else:
return heuristic(u, v)
def move_and_replan(self, robot_position: (int, int)):
path = [robot_position]
self.s_start = robot_position
self.s_last = self.s_start
self.compute_shortest_path()
while self.s_start != self.s_goal:
assert (self.rhs[self.s_start] !=
float('inf')), "There is no known path!"
succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False)
min_s = float('inf')
arg_min = None
for s_ in succ:
temp = self.c(self.s_start, s_) + self.g[s_]
if temp < min_s:
min_s = temp
arg_min = s_
self.s_start = arg_min
path.append(self.s_start)
changed_edges_with_old_cost = self.rescan()
if changed_edges_with_old_cost:
self.k_m += heuristic(self.s_last, self.s_start)
self.s_last = self.s_start
vertices = changed_edges_with_old_cost.vertices
for vertex in vertices:
v = vertex.pos
succ_v = vertex.edges_and_c_old
for u, c_old in succ_v.items():
c_new = self.c(u, v)
if c_old > c_new:
if u != self.s_goal:
self.rhs[u] = min(self.rhs[u],
self.c(u, v) + self.g[v])
elif self.rhs[u] == c_old + self.g[v]:
if u != self.s_goal:
min_s = float('inf')
succ_u = self.sensed_map.succ(vertex=u)
for s_ in succ_u:
temp = self.c(u, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[u] = min_s
self.update_vertex(u)
self.compute_shortest_path()
print("path found!")
return path, self.g, self.rhs
def ida_star(graph, start_id, end_id):
start_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == start_id
][0]
end_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == end_id
][0]
#Variaveis de estatisticas
start_time = timeit.default_timer()
expanded = [0]
cost = 0
visited = []
sum_visited = 0
branching_factor = []
success = False
fail = False
solution = []
limit = utils.heuristic(start_node, end_node)
while not success or not fail:
distance = ida_star_aux(graph, start_node, end_node, 0, visited, limit,
solution, expanded, branching_factor)
if distance == float("inf"):
fail = True
break
elif distance < 0:
success = True
break
else:
limit = distance
solution = []
sum_visited += len(visited)
visited = []
end_time = timeit.default_timer()
cost = -1 * distance
depth = len(solution) - 1
exec_time = end_time - start_time
average_branching_factor = sum(branching_factor) / len(branching_factor)
return [node.vertex_id for node in solution], depth, cost, expanded[
0], sum_visited, average_branching_factor, exec_time, "success" if success else "fail"
def a_star(graph):
n = graph.get_total_vertex_count()
s = graph.s
t = graph.t
cost = graph.cost
pq = []
f = [1e9] * n
g = [1e9] * n
h = heuristic(graph.point_list, graph.point_list[t])
p = [-1] * n
closed = [False] * n
g[s] = 0
f[s] = g[s] + h[s]
p[s] = -2
heappush(pq, (f[s], s))
while len(pq) > 0 and f[t] >= pq[0][0]:
f_u, u = heappop(pq)
if f_u > f[u]:
continue
closed[u] = True
for v in range(n):
if closed[v]:
continue
g[v] = min(g[v], g[u] + cost[u][v])
if f[v] > g[v] + h[v]:
f[v] = g[v] + h[v]
p[v] = u
heappush(pq, (f[v], v))
if p[t] == -1:
return False, []
path = []
v = t
while v != -2:
path.append(v)
v = p[v]
path.reverse()
return True, path
def __init__(self, map: OccupancyGridMap, s_start: (int, int),
s_goal: (int, int)):
self.new_edges_and_old_costs = None
self.s_start = s_start
self.s_goal = s_goal
self.s_last = s_start
self.k_m = 0
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
exploration_setting='8N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def ida_star_aux(graph, node, end_node, distance, visited, limit, path,
expanded, branching_factor):
path.append(node)
visited.append(node)
if node == end_node:
return -distance
estimate = distance + utils.heuristic(node, end_node)
if estimate > limit:
path.pop()
return estimate
n_limit = float("inf")
edges = list(graph[node].keys())
edges.sort(key=lambda edge: edge.vertex_id)
n = 0
expanded[0] += 1
for edge in edges:
if edge not in visited:
n += 1
edge_dist = ida_star_aux(graph, edge, end_node,
distance + graph[node][edge].weight,
visited, limit, path, expanded,
branching_factor)
if edge_dist < 0:
return edge_dist
elif edge_dist < n_limit:
n_limit = edge_dist
branching_factor.append(n)
if len(path) > 0:
path.pop()
return n_limit
def AStar(start: str, end: str, trasbordos: bool):
G = getAtenas() # Get the Godamm MAP
visitedList = [] # Nodes we alredy looked into
toVisit = PriorityQueue(
) # The Nodes to visit: get will return de min of all (f(n),count,node)
final = None
found = False
count = 0
currentLine = 0
lessChanges = False # Opcion para decir si queremos menos trasbordos posibles, google maps implementa algo similar
toVisit.put((0, 0, nodeState(start, None,
0))) # Obvious but add the start to toVisit
while (not toVisit.empty() and (not found)): # We have nodes to visit
lowest = toVisit.get() # Lowest value of f(n) from priority Queue
lowestVertex = lowest[
2] # Get the node with f(n) lowest nodeState(node,parent)
visitedList.append(
lowestVertex.node) # We visit the node, add it to the list
if (lowestVertex.node == end):
found = True # Yeay we have found it, lets stop
final = lowestVertex # Save the node for backtracking
else:
'''
Miramos si hemos cambiado de linea, de ese modo podremos calcular el coste real de pasar a la siguiente parada
'''
if (lessChanges and currentLine
not in G.nodes[lowestVertex.node]["lineas"]):
currentLine = G.nodes[lowestVertex.node]["lineas"][0]
children = [i for i in G.neighbors(lowestVertex.node)
] # All the children of the LowestVertex
for child in children: # Iterate through all children
count += 1
'''
f(n)=g(n)+h(n)
g(n): Distance between Parent & Child --> Stored in the Edge of the graph ++ Change of trains
h(n): How far are we from the end Node --> Need a function to calculate
'''
g = lowestVertex.distance + G.edges[(lowestVertex.node,
child)]["g(n)"]
change = False
if (lessChanges and currentLine not in G.nodes[child]["lineas"]
): #Change of metro line
g += 6.666 # Se aumenta G ya que el coste para pasar por esa arista es mayor al haber trasbordo
change = True # Si hemos cambiado de linea guardarlo para luego buscar en numero de trasbordos
f = g + heuristic(G, child, end) #Calculate f(n)=g(n)+h(n)
toInsert = nodeState(
child, lowestVertex, g, change
) # Create the tuple (Node,Parent,cost2getHere,changeOfTrain)
if (toInsert.node not in visitedList
): # Check if we have alredy been there (NO endless Loops)
toVisit.put(
(f, count, toInsert
)) # We will need further examination of the node
if (found): # Congrats we have found something
path, cost, changes = backTracking(final)
print(path)
print(cost)
print(changes)
else: # Something smells like a Bug in the Code..... Or the really Intelligent USER
print("No path could be found. ¯\_(ツ)_/¯ ")
def greedy(graph, start_id, end_id):
start_time = timeit.default_timer()
expanded = 0
branching_factor = []
start_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == start_id
][0]
end_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == end_id
][0]
parentMap = {}
visited = []
stack = []
solution = []
current = start_node
stack.append(current)
success = False
while stack:
current = stack[-1]
stack.pop()
if current not in visited:
visited.append(current)
if current == end_node:
success = True
break
else:
expanded += 1
n = 0
children = {}
for child in graph[current]:
if child not in visited:
children[child] = utils.heuristic(child, end_node)
parentMap[child] = current
n += 1
branching_factor.append(n)
if children:
sorted_children = list({
k: v
for k, v in sorted(children.items(),
key=lambda item: item[1])
}.keys())
sorted_children.reverse()
stack += sorted_children
cost = 0
if success:
while current != start_node:
solution.append(current.vertex_id)
cost += graph[current][parentMap[current]].weight
current = parentMap[current]
solution.append(current.vertex_id)
solution.reverse()
end_time = timeit.default_timer()
depth = len(solution) - 1
exec_time = end_time - start_time
average = sum(branching_factor) / len(branching_factor)
return solution, depth, cost, expanded, len(
visited), average, exec_time, 'success'
else:
end_time = timeit.default_timer()
exec_time = end_time - start_time
return solution, -1, cost, expanded, len(
visited), -1, exec_time, 'failure'
def algorithm(self, algorithm, color):
if self.start is None or self.goal is None:
self.curr_grid = self.clean_grid.copy()
return
self.reset_world('soft', new_run=True, keep_paths=True)
# avoid increment when algorithm is applied two times on the same world
if not self.applied_this_run[algorithm]:
self.algorithm_runs[algorithm] += 1
self.applied_this_run[algorithm] = True
start = self.start
goal = self.goal
h = heuristic(self.clean_grid, goal)
data_algo = {
'h': h,
'frontier': {start},
'inner': set(),
'g_score': {
start: 0
},
'f_score': {
start: h[start]
},
'come_from': {
start: None
}
}
steps = 0
self.curr_grid = self.clean_grid.copy()
while len(data_algo['frontier']) > 0:
if self.step_by_step:
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.type == pygame.QUIT:
sys.exit(0)
if event.key == self.control["R"]:
self.paths = []
self.curr_grid = self.clean_grid
return
# update only when SPACE is pressed
if event.type == pygame.KEYDOWN and event.key == self.control[
"SPACE"]:
# apply one step of the selected algorithm
data_algo, done = self.step(
data_algo, algorithm, color)
steps += 1
if done:
self.print_this, self.printed_infos = \
build_print_line(algorithm, steps, self.paths, self.run_num, self.printed_infos)
self.caption = self.print_this
return
else:
data_algo, done = self.step(data_algo, algorithm, color)
steps += 1
if done:
self.print_this, self.printed_infos = \
build_print_line(algorithm, steps, self.paths, self.run_num, self.printed_infos)
self.caption = self.print_this
self.update_screen()
if self.gif:
pygame.image.save(self.screen,
'./temp/{}.bmp'.format(steps))
create_gif(self.run_num, algorithm)
return
self.update_screen()
if self.gif:
pygame.image.save(self.screen, './temp/{}.bmp'.format(steps))
def c(self, u: (int, int), v: (int, int)) -> float:
if not self.sensed_map.is_unoccupied(
u) or not self.sensed_map.is_unoccupied(v):
return float('inf')
else:
return heuristic(u, v)
def calculate_key(self, s: (int, int)):
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start,
s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)
start = [0, 0]
goal1 = [len(map[0]) - 1, len(map) - 1]
goal2 = [4, 2]
goal = goal2
#Define the possible directions the robot can move
dirs = [
[-1, 0], # go up
[0, -1], # go left
[1, 0], # go down
[0, 1]
] # go right
dirs_name = ['^', '<', 'v', '>']
#Create heuristic map
heuristic = heuristic(map, goal, method='Manhattan', show=False)
# Output example:
# [6, 5, 4, 3, 2, 3]
# [5, 4, 3, 2, 1, 2]
# [4, 3, 2, 1, 0, 1]
# [5, 4, 3, 2, 1, 2]
# [6, 5, 4, 3, 2, 3]
expansion_grid = [[-1 for row in range(len(map[0]))]
for col in range(len(map))]
expansion_grid[goal[1]][goal[0]] = '*'
#Define blocked
#blocked corresponds to the limits of the map and the walls
def is_blocked(map, node):
def a_star(graph, start_id, end_id):
start_time = timeit.default_timer()
expanded = 0
branching_factor = []
start_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == start_id
][0]
end_node = [
node for node in list(graph.graph.keys()) if node.vertex_id == end_id
][0]
openList = {}
closedList = {}
# A lista contém respectivamente: g(custo acumulado), funcao h da heuristica e funcao f(g + h)
openList[start_node] = [
0,
utils.heuristic(start_node, end_node),
0 + utils.heuristic(start_node, end_node)
]
success = False
solution = []
parentMap = {}
while openList:
current = utils.find_smaller(openList, 'a_star')
closedList[current] = openList[current]
del openList[current]
if current == end_node:
success = True
break
else:
expanded += 1
n = 0
for child in graph[current]:
if child in closedList:
continue
if child in openList:
new_g = closedList[current][0] + graph[current][
child].weight
if openList[child][0] > new_g:
openList[child][0] = new_g
openList[child][2] = new_g + openList[child][1]
parentMap[child] = current
n += 1
else:
child_g = closedList[current][0] + graph[current][
child].weight
child_h = utils.heuristic(child, end_node)
openList[child] = [child_g, child_h, child_g + child_h]
parentMap[child] = current
n += 1
branching_factor.append(n)
cost = 0
if success:
while current != start_node:
solution.append(current.vertex_id)
cost += graph[current][parentMap[current]].weight
current = parentMap[current]
solution.append(current.vertex_id)
solution.reverse()
end_time = timeit.default_timer()
depth = len(solution) - 1
exec_time = end_time - start_time
average = sum(branching_factor) / len(branching_factor)
return solution, depth, cost, expanded, len(openList) + len(
closedList), average, exec_time, 'success'
else:
end_time = timeit.default_timer()
exec_time = end_time - start_time
return solution, -1, cost, expanded, len(
visited), -1, exec_time, 'failure'
def h(self, position):
rp = tuple(position)
if rp not in self.h_table:
self.h_table[rp] = heuristic(position, self.goal)
return self.h_table[rp]
