def Dijkstra(num_nodes, mission, f_next, heuristic=None, num_controls=0):
"""Djikstra planner."""
t = Timer()
t.tic()
unvis_node = -1
previous = np.full(num_nodes, dtype=np.int, fill_value=unvis_node)
cost_to_come = np.zeros(num_nodes)
control_to_come = np.zeros((num_nodes, num_controls), dtype=np.int)
startNode = mission['start']['id']
goalNode = mission['goal']['id']
#Priority queue based on the lower cost-to-go
q = PriorityQueue()
q.insert(0,startNode)
foundPlan = False
while not q.IsEmpty():
x_ctc = q.pop()
x = x_ctc[1]
if x == goalNode:
foundPlan = True
break
neighbours, u, d = f_next(x)
for xi, ui, di in zip(neighbours, u, d):
if previous[xi] == unvis_node or cost_to_come[xi] > cost_to_come[x] + di:
previous[xi] = x
cost_to_come[xi] = cost_to_come[x] + di
q.insert(cost_to_come[xi],xi)
if num_controls > 0:
control_to_come[xi] = ui
# Recreate the plan by traversing previous from goal node
if not foundPlan:
return []
else:
plan = [goalNode]
length = cost_to_come[goalNode]
control = []
while plan[0] != startNode:
if num_controls > 0:
control.insert(0, control_to_come[plan[0]])
plan.insert(0, previous[plan[0]])
return {'plan': plan,
'length': length,
'num_visited_nodes': np.sum(previous != unvis_node),
'name': 'Djikstra',
'time': t.toc(),
'control': control,
'visited_nodes': previous[previous != unvis_node]}
from queues import PriorityQueue
class Golfer:
def __init__(self, name, score):
self.name = name
self.score = score
def __str__(self):
return "{0:16}: {1}".format(self.name, self.score)
def __gt__(self, other):
return self.score < other.score # less is more
tiger = Golfer("Tiger Woods", 61)
phil = Golfer("Phil Mickelson", 72)
hal = Golfer("Hal Sutton", 69)
pq = PriorityQueue()
for g in [tiger, phil, hal]:
pq.insert(g)
while not pq.is_empty():
print(pq.remove())