You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
70 lines
1.6 KiB
70 lines
1.6 KiB
"""
|
|
Dijkstra 2D
|
|
@author: huiming zhou
|
|
"""
|
|
|
|
import os
|
|
import sys
|
|
import math
|
|
import heapq
|
|
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
"/../../Search_based_Planning/")
|
|
|
|
from Search_2D import plotting, env
|
|
|
|
from Search_2D.Astar import AStar
|
|
|
|
|
|
class Dijkstra(AStar):
|
|
"""Dijkstra set the cost as the priority
|
|
"""
|
|
def searching(self):
|
|
"""
|
|
Breadth-first Searching.
|
|
:return: path, visited order
|
|
"""
|
|
|
|
self.PARENT[self.s_start] = self.s_start
|
|
self.g[self.s_start] = 0
|
|
self.g[self.s_goal] = math.inf
|
|
heapq.heappush(self.OPEN,
|
|
(0, self.s_start))
|
|
|
|
while self.OPEN:
|
|
_, s = heapq.heappop(self.OPEN)
|
|
self.CLOSED.append(s)
|
|
|
|
if s == self.s_goal:
|
|
break
|
|
|
|
for s_n in self.get_neighbor(s):
|
|
new_cost = self.g[s] + self.cost(s, s_n)
|
|
|
|
if s_n not in self.g:
|
|
self.g[s_n] = math.inf
|
|
|
|
if new_cost < self.g[s_n]: # conditions for updating Cost
|
|
self.g[s_n] = new_cost
|
|
self.PARENT[s_n] = s
|
|
|
|
# best first set the heuristics as the priority
|
|
heapq.heappush(self.OPEN, (new_cost, s_n))
|
|
|
|
return self.extract_path(self.PARENT), self.CLOSED
|
|
|
|
|
|
def main():
|
|
s_start = (5, 5)
|
|
s_goal = (45, 25)
|
|
|
|
dijkstra = Dijkstra(s_start, s_goal, 'None')
|
|
plot = plotting.Plotting(s_start, s_goal)
|
|
|
|
path, visited = dijkstra.searching()
|
|
plot.animation(path, visited, "Dijkstra's") # animation generate
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|