def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: a tuple containing the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)] and its total cost
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
# Lambda function that calculates the distance between two adjacents nodes
h = lambda start, goal: start.distance_to(goal.i, goal.j)
# Cost of the path
cost = 0
while node is not None:
reversed_path.append(node.get_position())
# Here is added the distance between a node and its parent to the cost
cost += 0 if not node.parent else h(node,node.parent)
node = node.parent
# We return a tuple with the past and its cost
return reversed_path[::-1], cost # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
# Lambda function that represents the heuristic function to estimate a distance between two nodes
h = lambda start, goal: start.distance_to(goal.i, goal.j)
# Priority queue
pq = []
# Start and goal converted to the class Node
start, goal = self.node_grid.grid[start_position], self.node_grid.grid[goal_position]
# The distance from a node to itself is zero
start.g = 0
# First element added to the heap is the first node
heapq.heappush(pq, (start.g, start))
while pq:
# We pop the closest element in the heap
cost, node = heapq.heappop(pq)
# We close this node
node.closed = True
for sucessor in self.node_grid.get_successors(node.i, node.j):
# Convert sucessor to the class Node
sucessor = self.node_grid.grid[sucessor]
# If the node wasn't already pushed into the heap, we test it
if not sucessor.closed:
# Close it
sucessor.closed = True
# Update its distance
if sucessor.g > cost + h(node, sucessor):
sucessor.g = cost + h(node, sucessor)
sucessor.parent = node
# Push it into the heap
heapq.heappush(pq, (sucessor.g, sucessor))
# Get path
ans = self.construct_path(goal)
self.node_grid.reset()
return ans
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
# Lambda function that represents the heuristic function to estimate a distance between two nodes
h = lambda start, goal: start.distance_to(goal.i, goal.j)
# Priority queue
pq = []
# Start and goal converted to the class Node
start, goal = self.node_grid.grid[start_position], self.node_grid.grid[goal_position]
# Estimates answer
start.g = h(start, goal)
# Push it into the heap
heapq.heappush(pq, (start.g, start))
while pq:
# Pop the closest
cost, node = heapq.heappop(pq)
node.closed = True
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if not sucessor.closed:
sucessor.closed = True
sucessor.parent = node
if sucessor == goal:
ans = self.construct_path(goal)
self.node_grid.reset()
return ans
sucessor.g = h(sucessor, goal)
heapq.heappush(pq, (sucessor.g, sucessor))
ans = self.construct_path(goal)
self.node_grid.reset()
return ans
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
# Lambda function that represents the heuristic function to estimate a distance between two nodes
h = lambda start, goal: start.distance_to(goal.i, goal.j)
# Priority queue
pq = []
# Start and goal converted to the class Node
start, goal = self.node_grid.grid[start_position], self.node_grid.grid[goal_position]
# h(n) + g(n)
start.f = h(start, goal)
# The distance from a node to itself is zero
start.g = 0
heapq.heappush(pq, (start.f, start))
while pq:
cost, node = heapq.heappop(pq)
node.closed = True
if node == goal:
ans = self.construct_path(goal)
self.node_grid.reset()
return ans
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if not sucessor.closed:
# Use the estimative to update distance
if sucessor.f > node.g + h(node,sucessor) + h(sucessor, goal):
sucessor.g = node.g + h(node, sucessor)
sucessor.f = sucessor.g + h(sucessor,goal)
sucessor.parent = node
heapq.heappush(pq, (sucessor.f, sucessor))
ans = self.construct_path(goal)
self.node_grid.reset()
return ans
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
#pegando os nos
node_inicio = self.node_grid.get_node(start_position[0],
start_position[1])
node_final = self.node_grid.get_node(goal_position[0],
goal_position[1])
#prioridade do no inicial eh 0
node_inicio.f = 0
#criando fila de prioridade
pq = []
#inserindo no inicial na fila de prioridade
heapq.heappush(pq, (node_inicio.f, node_inicio))
while pq != []:
#tira o menor no
prior, node = heapq.heappop(pq)
#visita o no
node.closed = True
# se for o no final, termina
if node == node_final:
break
#pega os sucessores
sucessores = self.node_grid.get_successors(node.i, node.j)
#calcula a prioridade de cada um e insere na fila de prioridades
tupla_node = (node.i, node.j)
for tupla_filho in sucessores:
filho = self.node_grid.get_node(tupla_filho[0], tupla_filho[1])
if not filho.closed:
# custo do inicio ate o filho
custo = node.f + self.cost_map.get_edge_cost(
tupla_node, tupla_filho)
if (filho.f > custo):
# se f do filho for maior que o (f do pai + custo), troca o f e o parent
filho.f = custo
filho.parent = node
#insere filho na fila de prioridade
heapq.heappush(pq, (filho.f, filho))
return self.construct_path(node_final), node_final.f
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
# pegando os nos
node_inicio = self.node_grid.get_node(start_position[0],
start_position[1])
node_final = self.node_grid.get_node(goal_position[0],
goal_position[1])
# f -> prioridade
# g -> custo
#setando prioridade e custo inicial
node_inicio.f = 0
node_inicio.g = node_inicio.distance_to(node_final.i, node_final.j)
# criando fila de prioridade
pq = []
# inserindo noh inicial na fila de prioridade
heapq.heappush(pq, (node_inicio.f, node_inicio))
node_inicio.closed = True
while pq != []:
# tira o menor no
prior, node = heapq.heappop(pq)
tupla_node = (node.i, node.j)
# visita o no
#node.closed = True
#pega os sucessores
sucessores = self.node_grid.get_successors(node.i, node.j)
#calcula a prioridade de cada um e insere na fila de prioridades
for tupla_filho in sucessores:
filho = self.node_grid.get_node(tupla_filho[0], tupla_filho[1])
if not filho.closed:
#setando pai
filho.parent = node
#setando prioridade
filho.g = filho.distance_to(node_final.i, node_final.j)
#setando custo
filho.f = node.f + self.cost_map.get_edge_cost(
tupla_node, tupla_filho)
if filho == node_final:
return self.construct_path(node_final), node_final.f
# insere filho na fila de prioridade
heapq.heappush(pq, (filho.g, filho))
#visita o filho
filho.closed = True
return self.construct_path(node_final), node_final.f
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
# pegando os nos
node_inicio = self.node_grid.get_node(start_position[0],
start_position[1])
node_final = self.node_grid.get_node(goal_position[0],
goal_position[1])
# f -> prioridade
# g -> custo
# setando prioridade e custo inicial
node_inicio.f = node_inicio.distance_to(node_final.i, node_final.j)
node_inicio.g = 0
# criando fila de prioridade
pq = []
# inserindo noh inicial na fila de prioridade
heapq.heappush(pq, (node_inicio.f, node_inicio))
while pq != []:
# tira o menor no
prior, node = heapq.heappop(pq)
if node == node_final:
return self.construct_path(node_final), node_final.f
tupla_node = (node.i, node.j)
# visita o no
node.closed = True
# pega os sucessores
sucessores = self.node_grid.get_successors(node.i, node.j)
# calcula a prioridade de cada um e insere na fila de prioridades
for tupla_filho in sucessores:
filho = self.node_grid.get_node(tupla_filho[0], tupla_filho[1])
if not filho.closed:
newG = node.g + self.cost_map.get_edge_cost(
tupla_node, tupla_filho)
dist = filho.distance_to(node_final.i, node_final.j)
if filho.f > newG + dist:
filho.g = newG
filho.f = filho.g + dist
# setando pai
filho.parent = node
# insere filho na fila de prioridade
heapq.heappush(pq, (filho.f, filho))
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset() #seta todas as distâncias para infinito
i = start_position[0]
j = start_position[1]
self.node_grid.grid[i, j].f = 0
pq = []
heapq.heappush(
pq, (self.node_grid.grid[i, j].f, self.node_grid.grid[i, j]))
while True:
davez = Node(300, 300)
while len(pq) != 0:
atual = pq.pop(0)
if atual[1].closed == False:
davez = atual[1]
break
if davez.i == 300:
break
self.node_grid.grid[davez.i, davez.j].closed = True
t = davez.get_position()
p = self.node_grid.get_successors(t[0], t[1])
for item in p:
n = (davez.i, davez.j)
m = (self.node_grid.grid[item[0], item[1]].i,
self.node_grid.grid[item[0], item[1]].j)
dist = self.cost_map.get_edge_cost(n, m)
h = item[0]
k = item[1]
if self.node_grid.grid[
h, k].f > davez.f + dist and self.node_grid.grid[
h, k].closed == False:
self.node_grid.grid[h, k].parent = davez
self.node_grid.grid[h, k].f = davez.f + dist
heapq.heappush(pq, (self.node_grid.grid[h, k].f,
self.node_grid.grid[h, k]))
pt = self.construct_path(self.node_grid.grid[goal_position[0],
goal_position[1]])
ct = self.node_grid.grid[goal_position[0], goal_position[1]].f
return pt, ct
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset() #seta todas as distâncias para infinito
i = start_position[0]
booleana = 1
j = start_position[1]
self.node_grid.grid[i, j].f = 0
pq = []
heapq.heappush(
pq, (self.node_grid.grid[i, j].f, self.node_grid.grid[i, j]))
while len(pq) != 0:
atual = heapq.heappop(pq)
davez = atual[1]
self.node_grid.grid[davez.i, davez.j].closed = True
t = davez.get_position()
p = self.node_grid.get_successors(t[0], t[1])
for item in p:
node = self.node_grid.grid[item[0], item[1]]
dist = node.distance_to(goal_position[0], goal_position[1])
n = (davez.i, davez.j)
m = (self.node_grid.grid[item[0], item[1]].i,
self.node_grid.grid[item[0], item[1]].j)
distg = self.cost_map.get_edge_cost(n, m)
h = item[0]
k = item[1]
if node.closed == False:
self.node_grid.grid[h, k].parent = davez
self.node_grid.grid[h, k].f = davez.f + distg
self.node_grid.grid[h, k].closed = True
heapq.heappush(pq, (dist, self.node_grid.grid[h, k]))
if node.i == goal_position[0] and node.j == goal_position[1]:
booleana = 0
break
if booleana == 0:
break
pt = self.construct_path(self.node_grid.grid[goal_position[0],
goal_position[1]])
ct = self.node_grid.grid[goal_position[0], goal_position[1]].f
return pt, ct
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
i = start_position[0]
j = start_position[1]
self.node_grid.grid[i, j].f = self.node_grid.grid[i, j].distance_to(
goal_position[0], goal_position[1])
self.node_grid.grid[i, j].g = 0
pq = []
heapq.heappush(
pq, (self.node_grid.grid[i, j].f, self.node_grid.grid[i, j]))
while len(pq) != 0:
atual = pq.pop(0)
davez = atual[1]
self.node_grid.grid[davez.i, davez.j].closed = True
t = davez.get_position()
p = self.node_grid.get_successors(t[0], t[1])
if davez.i == goal_position[0] and davez.j == goal_position[1]:
break
for item in p:
n = (davez.i, davez.j)
m = (self.node_grid.grid[item[0], item[1]].i,
self.node_grid.grid[item[0], item[1]].j)
distg = self.cost_map.get_edge_cost(n, m)
node = self.node_grid.grid[item[0], item[1]]
dist = node.distance_to(goal_position[0], goal_position[1])
h = item[0]
k = item[1]
if node.f > davez.g + dist + distg:
self.node_grid.grid[h, k].parent = davez
self.node_grid.grid[h, k].f = davez.g + dist + distg
self.node_grid.grid[h, k].g = davez.g + distg
heapq.heappush(pq, (self.node_grid.grid[h, k].f,
self.node_grid.grid[h, k]))
pt = self.construct_path(self.node_grid.grid[goal_position[0],
goal_position[1]])
ct = self.node_grid.grid[goal_position[0], goal_position[1]].f
return pt, ct
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
start_node = self.node_grid.get_node(start_position[0],
start_position[1])
goal_node = self.node_grid.get_node(goal_position[0], goal_position[1])
pq = []
start_node.f = 0
heapq.heappush(pq, (start_node.f, start_node))
while pq:
f, current_node = heapq.heappop(pq)
current_node.closed = True
if current_node == goal_node:
break
current_node_i, current_node_j = current_node.get_position()
for successor in self.node_grid.get_successors(
current_node_i, current_node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if not successor_node.closed and \
successor_node.f > current_node.f + self.cost_map.get_edge_cost(current_node.get_position(),
successor_node.get_position()):
successor_node.f = current_node.f + self.cost_map.get_edge_cost(
current_node.get_position(),
successor_node.get_position())
# print("custo: "+str(successor_node.f))
successor_node.parent = current_node
heapq.heappush(pq, (successor_node.f, successor_node))
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
path = self.construct_path(goal_node)
total_cost = goal_node.f
self.node_grid.reset()
return path, total_cost # Feel free to change this line of code
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
start_node = self.node_grid.get_node(start_position[0],
start_position[1])
goal_node = self.node_grid.get_node(goal_position[0], goal_position[1])
pq = []
start_node.g = start_node.distance_to(goal_node.get_position()[0],
goal_node.get_position()[1])
start_node.f = start_node.g
heapq.heappush(pq, (start_node.g, start_node))
finished = False
while pq and not finished:
g, current_node = heapq.heappop(pq)
current_node.closed = True
current_node_i, current_node_j = current_node.get_position()
for successor in self.node_grid.get_successors(
current_node_i, current_node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if not successor_node.closed:
successor_node.closed = True
successor_node.parent = current_node
if current_node == goal_node:
finished = True
break
successor_node.g = successor_node.distance_to(
goal_node.get_position()[0],
goal_node.get_position()[1])
successor_node.f = current_node.f + self.cost_map.get_edge_cost(
current_node.get_position(),
successor_node.get_position())
heapq.heappush(pq, (successor_node.g, successor_node))
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
path = self.construct_path(goal_node)
total_cost = goal_node.f
self.node_grid.reset()
return path, total_cost # Feel free to change this line of code
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
start_node = self.node_grid.get_node(start_position[0],
start_position[1])
goal_node = self.node_grid.get_node(goal_position[0], goal_position[1])
pq = []
start_node.g = 0
start_node.f = start_node.distance_to(goal_node.get_position()[0],
goal_node.get_position()[1])
heapq.heappush(pq, (start_node.f, start_node))
while pq:
f, current_node = heapq.heappop(pq)
current_node.closed = True
if current_node == goal_node:
break
current_node_i, current_node_j = current_node.get_position()
for successor in self.node_grid.get_successors(
current_node_i, current_node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if not successor_node.closed and \
successor_node.f > current_node.g +\
self.cost_map.get_edge_cost(current_node.get_position(), successor_node.get_position()) +\
successor_node.distance_to(goal_node.get_position()[0], goal_node.get_position()[1]):
successor_node.g = current_node.g + self.cost_map.get_edge_cost(
current_node.get_position(),
successor_node.get_position())
successor_node.f = successor_node.g + successor_node.distance_to(
goal_node.get_position()[0],
goal_node.get_position()[1])
successor_node.parent = current_node
heapq.heappush(pq, (successor_node.f, successor_node))
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
path = self.construct_path(goal_node)
total_cost = goal_node.f
self.node_grid.reset()
return path, total_cost # Feel free to change this line of code
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
pq = []
start = self.node_grid.grid[start_position[0], start_position[1]]
start.f = 0
goal = self.node_grid.grid[goal_position[0], goal_position[1]]
heapq.heappush(pq, (start.f, start))
while not (len(pq) == 0):
f, node = heapq.heappop(pq)
node.closed = True
if (node.i == goal_position[0]) and (node.j == goal_position[1]):
goal.f = goal.parent.f + self.cost_map.get_edge_cost(
goal, goal.parent)
break
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if (sucessor.f >
node.f + self.cost_map.get_edge_cost(node, sucessor)):
sucessor.f = node.f + self.cost_map.get_edge_cost(
node, sucessor)
sucessor.parent = node
heapq.heappush(pq, (sucessor.f, sucessor))
path = self.construct_path(goal)
cost = goal.f
self.node_grid.reset()
return path, cost
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
pq = []
start = self.node_grid.grid[start_position[0], start_position[1]]
start.f = Node.distance_to(start, goal_position[0], goal_position[1])
start.g = 0
goal = self.node_grid.grid[goal_position[0], goal_position[1]]
heapq.heappush(pq, (start.f, start))
while not (len(pq) == 0):
f, node = heapq.heappop(pq)
node.closed = True
if (node.i == goal_position[0]) and (node.j == goal_position[1]):
goal.f = goal.parent.f + self.cost_map.get_edge_cost(
goal, goal.parent)
break
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if not (sucessor.closed == True):
sucessor.parent = node
sucessor.f = Node.distance_to(goal, sucessor.i, sucessor.j)
sucessor.g = sucessor.parent.g + self.cost_map.get_edge_cost(
sucessor, sucessor.parent)
heapq.heappush(pq, (sucessor.f, sucessor))
self.node_grid.get_node(sucessor.i,
sucessor.j).closed = True
path = self.construct_path(goal)
cost = goal.g
self.node_grid.reset()
return path, cost
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
pq = []
goal = self.node_grid.grid[goal_position[0], goal_position[1]]
start = self.node_grid.grid[start_position[0], start_position[1]]
start.g = 0
start.f = Node.distance_to(start, goal_position[0], goal_position[1])
heapq.heappush(pq, (start.f, start))
while not (len(pq) == 0):
f, node = heapq.heappop(pq)
node.closed = True
if (node.i == goal_position[0]) and (node.j == goal_position[1]):
goal.g = goal.parent.g + self.cost_map.get_edge_cost(
goal, goal.parent)
goal.f = goal.g + 0
break
for sucessor in self.node_grid.get_successors(node.i, node.j):
sucessor = self.node_grid.grid[sucessor]
if (sucessor.f >
node.g + self.cost_map.get_edge_cost(node, sucessor) +
Node.distance_to(sucessor, goal_position[0],
goal_position[1])):
sucessor.g = node.g + self.cost_map.get_edge_cost(
node, sucessor)
sucessor.f = sucessor.g + Node.distance_to(
sucessor, goal_position[0], goal_position[1])
sucessor.parent = node
heapq.heappush(pq, (sucessor.f, sucessor))
path = self.construct_path(goal)
cost = goal.f
self.node_grid.reset()
return path, cost
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
node = self.node_grid.get_node(*start_position)
node.g = 0
pq = []
heapq.heappush(pq, (node.g, node))
while len(pq) != 0:
node.g, node = heapq.heappop(pq)
if node.get_position() == goal_position:
break
node.closed = True
for successor in self.node_grid.get_successors(*node.get_position()):
successor_node = self.node_grid.get_node(*successor)
if successor_node.closed == False:
if successor_node.g > node.g + self.cost_map.get_edge_cost(node.get_position(), successor):
successor_node.g = node.g + self.cost_map.get_edge_cost(node.get_position(), successor)
successor_node.parent = node
heapq.heappush(pq, (successor_node.g, successor_node))
return self.construct_path(node), node.g
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
node = self.node_grid.get_node(*start_position)
successor_node = self.node_grid.get_node(*start_position)
node.g = 0
pq = []
heapq.heappush(pq, (node.distance_to(*goal_position), node))
out = False
while len(pq) != 0 and out == False:
h, node = heapq.heappop(pq)
if node.get_position() == goal_position:
break
node.closed = True
for successor in self.node_grid.get_successors(*node.get_position()):
successor_node = self.node_grid.get_node(*successor)
successor_node.g = node.g + self.cost_map.get_edge_cost(node.get_position(), successor)
if successor_node.closed == False:
successor_node.parent = node
if successor_node.get_position == goal_position:
out = True
break
heapq.heappush(pq, (successor_node.distance_to(*goal_position), successor_node))
return self.construct_path(successor_node), successor_node.g
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
# The first return is the path as sequence of tuples (as returned by the method construct_path())
# The second return is the cost of the path
self.node_grid.reset()
node = self.node_grid.get_node(*start_position)
successor_node = self.node_grid.get_node(*start_position)
node.g = 0
node.f = node.distance_to(*goal_position)
pq = []
heapq.heappush(pq, (node.f, node))
while len(pq) != 0:
h, node = heapq.heappop(pq)
if node.get_position() == goal_position:
break
node.closed = True
for successor in self.node_grid.get_successors(*node.get_position()):
successor_node = self.node_grid.get_node(*successor)
if successor_node.f > node.g + self.cost_map.get_edge_cost(node.get_position(), successor) + successor_node.distance_to(*goal_position):
successor_node.g = node.g + self.cost_map.get_edge_cost(node.get_position(), successor)
successor_node.f = successor_node.g + successor_node.distance_to(*goal_position)
successor_node.parent = node
heapq.heappush(pq, (successor_node.f, successor_node))
return self.construct_path(successor_node), successor_node.f
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
goal_node = None
# inicializa a raiz:
start = self.node_grid.grid[start_position]
start.g = 0
start.closed = False # Open to enter the loop. There it will be closed
# inicializa heap:
sorted_nodes = []
heapq.heappush(sorted_nodes, (start.g, start))
while len(sorted_nodes) != 0:
_, actual_node = heapq.heappop(sorted_nodes)
if not actual_node.closed:
actual_node.closed = True
if actual_node.get_position() == goal_position:
goal_node = actual_node
break
actual_position = actual_node.get_position()
for successor_position in self.node_grid.get_successors(
actual_position[0], actual_position[1]):
successor = self.node_grid.grid[successor_position]
cost_actual_node_to_successor = self.cost_map.get_edge_cost(
actual_position, successor_position)
if successor.g > actual_node.g + cost_actual_node_to_successor:
successor.parent = actual_node
successor.g = actual_node.g + cost_actual_node_to_successor
heapq.heappush(sorted_nodes, (successor.g, successor))
return self.construct_path(goal_node), goal_node.g
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
goal_node = None
# inicializa a raiz:
start = self.node_grid.grid[start_position]
start.g = 0
start.h = start.distance_to(goal_position[0], goal_position[1])
start.closed = False # aberto para entrar no loop. La sera fechado
# inicializa heap:
sorted_nodes = []
heapq.heappush(sorted_nodes, (start.h, start))
while len(sorted_nodes) != 0:
_, actual_node = heapq.heappop(sorted_nodes)
if not actual_node.closed:
actual_node.closed = True
if actual_node.get_position() == goal_position:
goal_node = actual_node
break
actual_position = actual_node.get_position()
for successor_position in self.node_grid.get_successors(
actual_position[0], actual_position[1]):
successor = self.node_grid.grid[successor_position]
if not successor.closed:
successor.parent = actual_node
successor.h = successor.distance_to(
goal_position[0], goal_position[1])
cost_actual_node_to_successor = self.cost_map.get_edge_cost(
actual_position, successor_position)
successor.g = actual_node.g + cost_actual_node_to_successor
heapq.heappush(sorted_nodes, (successor.h, successor))
return self.construct_path(goal_node), goal_node.g
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
goal_node = None
# inicializa a raiz:
start = self.node_grid.grid[start_position]
start.g = 0
start.h = start.distance_to(goal_position[0], goal_position[1])
start.f = start.g + start.h
start.closed = False # aberto para entrar no loop. La sera fechado
# inicializa heap:
sorted_nodes = []
heapq.heappush(sorted_nodes, (start.f, start))
while len(sorted_nodes) != 0:
_, actual_node = heapq.heappop(sorted_nodes)
if not actual_node.closed:
actual_node.closed = True
if actual_node.get_position() == goal_position:
goal_node = actual_node
break
actual_position = actual_node.get_position()
for successor_position in self.node_grid.get_successors(
actual_position[0], actual_position[1]):
successor = self.node_grid.grid[successor_position]
cost_actual_node_to_successor = self.cost_map.get_edge_cost(
actual_position, successor_position)
successor.h = successor.distance_to(
goal_position[0], goal_position[1])
if successor.f > actual_node.g + cost_actual_node_to_successor + successor.h:
successor.parent = actual_node
successor.g = actual_node.g + cost_actual_node_to_successor
successor.f = successor.g + successor.h
heapq.heappush(sorted_nodes, (successor.f, successor))
return self.construct_path(goal_node), goal_node.g
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Dijkstra algorithm
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
goal = self.node_grid.get_node(goal_position[0], goal_position[1])
start.f = 0
heapq.heappush(pq, (start.f, start))
while pq:
f, node = heapq.heappop(pq)
node.closed = True
node_i, node_j = node.get_position()
if node == goal:
path_dijkstra = self.construct_path(goal)
cost_dijkstra = goal.f
self.node_grid.reset()
return path_dijkstra, cost_dijkstra
for successor in self.node_grid.get_successors(node_i, node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if not successor_node.closed:
if successor_node.f > node.f + self.cost_map.get_edge_cost(
node.get_position(),
successor_node.get_position()):
successor_node.f = node.f + self.cost_map.get_edge_cost(
node.get_position(), successor_node.get_position())
successor_node.parent = node
heapq.heappush(pq, (successor_node.f, successor_node))
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the Greedy Search algorithm
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
goal = self.node_grid.get_node(goal_position[0], goal_position[1])
start.g = start.distance_to(goal_position[0], goal_position[1])
start.f = 0
heapq.heappush(pq, (start.g, start))
while pq:
g, node = heapq.heappop(pq)
node.closed = True
node_i, node_j = node.get_position()
for successor in self.node_grid.get_successors(node_i, node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if not successor_node.closed:
successor_node.closed = True
successor_node.parent = node
successor_node.f = node.f + self.cost_map.get_edge_cost(
node.get_position(), successor_node.get_position())
if successor_node == goal:
path_greedy = self.construct_path(goal)
cost_greedy = goal.f
self.node_grid.reset()
return path_greedy, cost_greedy
successor_node.g = successor_node.distance_to(
goal_position[0], goal_position[1])
heapq.heappush(pq, (successor_node.g, successor_node))
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
# Todo: implement the A* algorithm
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
goal = self.node_grid.get_node(goal_position[0], goal_position[1])
start.g = 0
start.f = start.distance_to(goal_position[0], goal_position[1])
heapq.heappush(pq, (start.f, start))
while pq:
f, node = heapq.heappop(pq)
node.closed = True
node_i, node_j = node.get_position()
if node == goal:
path_a_star = self.construct_path(goal)
cost_a_star = goal.f
self.node_grid.reset()
return path_a_star, cost_a_star
for successor in self.node_grid.get_successors(node_i, node_j):
successor_node = self.node_grid.get_node(
successor[0], successor[1])
if successor_node.f > node.g + self.cost_map.get_edge_cost(
node.get_position(), successor_node.get_position(
)) + successor_node.distance_to(
goal_position[0], goal_position[1]):
successor_node.g = node.g + self.cost_map.get_edge_cost(
node.get_position(), successor_node.get_position())
successor_node.f = successor_node.g + successor_node.distance_to(
goal_position[0], goal_position[1])
successor_node.parent = node
heapq.heappush(pq, (successor_node.f, successor_node))
class PathPlanner(object):
"""
Represents a path planner, which may use Dijkstra, Greedy Search or A* to plan a path.
"""
def __init__(self, cost_map):
"""
Creates a new path planner for a given cost map.
:param cost_map: cost used in this path planner.
:type cost_map: CostMap.
"""
self.cost_map = cost_map
self.node_grid = NodeGrid(cost_map)
@staticmethod
def construct_path(goal_node):
"""
Extracts the path after a planning was executed.
:param goal_node: node of the grid where the goal was found.
:type goal_node: Node.
:return: the path as a sequence of (x, y) positions: [(x1,y1),(x2,y2),(x3,y3),...,(xn,yn)].
:rtype: list of tuples.
"""
node = goal_node
# Since we are going from the goal node to the start node following the parents, we
# are transversing the path in reverse
reversed_path = []
while node is not None:
reversed_path.append(node.get_position())
node = node.parent
return reversed_path[::-1] # This syntax creates the reverse list
def dijkstra(self, start_position, goal_position):
"""
Plans a path using the Dijkstra algorithm.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
start.f = 0
heapq.heappush(pq, (start.f, start))
# while pq is not empty
while len(pq) > 0:
# extract min
node = heapq.heappop(pq)[1]
# close node as its cost is defined
node.closed = True
# if node is the goal, stop searching
if node.i == goal_position[0] and node.j == goal_position[1]:
return self.construct_path(node), node.f
# otherwise, search successors
for successor in self.node_grid.get_successors(node.i, node.j):
# get successor node
succ = self.node_grid.get_node(successor[0], successor[1])
# if successor is not closed
if not succ.closed:
edge_cost = self.cost_map.get_edge_cost(node.get_position(), succ.get_position())
# if previous cost is bigger than current one, update successor info in pq
if succ.f > node.f + edge_cost:
succ.f = node.f + edge_cost
succ.parent = node
heapq.heappush(pq, (succ.f, succ))
# if there is no path to goal, return empty path and infinite cost
return [], inf
def greedy(self, start_position, goal_position):
"""
Plans a path using greedy search.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
start.g = 0 # cost until current node
start.f = start.distance_to(goal_position[0], goal_position[1]) # greedy search cost
# close node as its cost won't be updated (it will always be node's distance to goal)
start.closed = True
heapq.heappush(pq, (start.f, start))
# while pq is not empty
while len(pq) > 0:
# extract min
node = heapq.heappop(pq)[1]
# search successors
for successor in self.node_grid.get_successors(node.i, node.j):
# get successor node
succ = self.node_grid.get_node(successor[0], successor[1])
# if successor is not closed
if not succ.closed:
succ.parent = node
edge_cost = self.cost_map.get_edge_cost(node.get_position(), succ.get_position())
succ.g = node.g + edge_cost # cost until current node
# if node is the goal, stop searching
if succ.i == goal_position[0] and succ.j == goal_position[1]:
return self.construct_path(succ), succ.g
# otherwise, insert successor in pq
succ.f = succ.distance_to(goal_position[0], goal_position[1]) # greedy search cost
succ.closed = True # close node as its cost won't be updated (it will always be node's distance to goal)
heapq.heappush(pq, (succ.f, succ))
# if there is no path to goal, return empty path and infinite cost
return [], inf
def a_star(self, start_position, goal_position):
"""
Plans a path using A*.
:param start_position: position where the planning stars as a tuple (x, y).
:type start_position: tuple.
:param goal_position: goal position of the planning as a tuple (x, y).
:type goal_position: tuple.
:return: the path as a sequence of positions and the path cost.
:rtype: list of tuples and float.
"""
self.node_grid.reset()
pq = []
start = self.node_grid.get_node(start_position[0], start_position[1])
start.g = 0 # cost until current node
start.f = start.distance_to(goal_position[0], goal_position[1]) # A* cost
heapq.heappush(pq, (start.f, start))
# while pq is not empty
while len(pq) > 0:
# extract min
node = heapq.heappop(pq)[1]
# close node as its cost is defined
node.closed = True
# if node is the goal, stop searching
if node.i == goal_position[0] and node.j == goal_position[1]:
return self.construct_path(node), node.f
# search successors
for successor in self.node_grid.get_successors(node.i, node.j):
# get successor node
succ = self.node_grid.get_node(successor[0], successor[1])
# if successor is not closed
if not succ.closed:
edge_cost = self.cost_map.get_edge_cost(node.get_position(), succ.get_position())
h_cost = succ.distance_to(goal_position[0], goal_position[1]) # estimated cost from successor to goal
# if previous A* cost is bigger than current one, update node info in pq
if succ.f > node.g + edge_cost + h_cost:
succ.g = node.g + edge_cost # cost until current node
succ.f = succ.g + h_cost # A* cost
succ.parent = node
heapq.heappush(pq, (succ.f, succ))
# if there is no path to goal, return empty path and infinite cost
return [], inf
