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