def astar(sourceNode, destNode):
queue = PriorityQueue()
queue.put(sourceNode, 0)
previous = {sourceNode: None}
distance = {sourceNode: 0}
while not queue.empty():
current = queue.get()
if current == destNode:
#return path src -> dest
path = []
n = destNode
while n:
path.insert(0, n)
n = previous[n]
return path
for next in current.neighbors:
new_cost = distance[current] + 1
if next not in distance or new_cost < distance[next]:
distance[next] = new_cost
priority = new_cost + heuristic(destNode, next)
queue.put(next, priority)
previous[next] = current
return None
def prim(G):
cost = {}
parent = {}
u = None
P = PriorityQueue()
for v in G.vertexes:
if u is None:
u = v
cost[v] = float('inf')
P.add(float('inf'), v)
parent[v] = None
cost[u] = 0
P.change_priority(u, 0)
for i in P.Q:
print(i)
while not P.isEmpty():
print('wtf')
v_ele = P.get_min()
vertex = v_ele.data
print('minimum', v_ele)
for u, v, w in G.get_all_vertex(vertex):
print(u, v, w)
if P.check_ele(v) and cost[v] > cost[u] + w:
cost[v] = cost[u] + w
parent[v] = u
P.change_priority(v, cost[v])
print(cost)
print(parent)
def shortest_path(graph, sourceVertex):
min_heap = PriorityQueue(True)
distance = {}
parent = {}
for vertex in graph.all_vertex.values():
min_heap.add_task(sys.maxsize, vertex)
min_heap.change_task_priority(0, sourceVertex)
distance[sourceVertex] = 0
parent[sourceVertex] = None
while min_heap.is_empty() is False:
task = min_heap.peek_task()
weight = min_heap.get_task_priority(task)
current = min_heap.pop_task()
distance[current] = weight
for edge in current.edges:
adjacent = get_other_vertex_for_edge(current, edge)
if min_heap.contains_task(adjacent) is False:
continue
new_distance = distance[current] + edge.weight;
if min_heap.get_task_priority(adjacent) > new_distance:
min_heap.change_task_priority(new_distance, adjacent)
parent[adjacent] = current
return distance
def minimum_spanning_tree(graph):
min_heap = PriorityQueue(True)
vertex_to_edge = {}
result = []
for vertex in graph.all_vertex.values():
min_heap.add_task(sys.maxsize, vertex)
start_vertex = next(iter((graph.all_vertex.values())))
min_heap.change_task_priority(0, start_vertex)
while min_heap.is_empty() is False:
current = min_heap.pop_task()
if (current in vertex_to_edge):
spanning_tree_edge = vertex_to_edge[current]
result.append(spanning_tree_edge)
for edge in current.edges:
adjacent = get_other_vertex_for_edge(current, edge)
if min_heap.contains_task(
adjacent) is True and min_heap.get_task_priority(
adjacent) > edge.weight:
min_heap.change_task_priority(edge.weight, adjacent)
vertex_to_edge[adjacent] = edge
return result
def test_dequeue(self):
p = PriorityQueue()
assert p.heap.items == []
p.enqueue('A', 2)
assert p.heap.items == [(2, 'A')]
p.enqueue('B', 2)
assert p.heap.items == [(2, 'A'), (2, 'B')]
p.enqueue('C', 1)
assert p.heap.items == [(1, 'C'), (2, 'B'), (2, 'A')]
p.enqueue('D', 1)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B')]
p.enqueue('E', 3)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B'),
(3, 'E')]
p.enqueue('F', 2)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B'),
(3, 'E'), (2, 'F')]
assert p.dequeue() == 'C'
assert p.heap.items == [(1, 'D'), (2, 'B'), (2, 'A'), (2, 'F'),
(3, 'E')]
assert p.dequeue() == 'D'
assert p.heap.items == [(2, 'A'), (2, 'B'), (3, 'E'), (2, 'F')]
assert p.dequeue() == 'A'
assert p.heap.items == [(2, 'B'), (2, 'F'), (3, 'E')]
assert p.dequeue() == 'B'
assert p.heap.items == [(2, 'F'), (3, 'E')]
assert p.dequeue() == 'F'
assert p.heap.items == [(3, 'E')]
assert p.dequeue() == 'E'
assert p.heap.items == []
with self.assertRaises(ValueError):
p.dequeue()
def demo_priority_queue():
print('DEMO OF USING PRIORITY QUEUE WITH DIFFERENT TYPES')
key_type = random.choice([random_int, random_str])
puts_num = random.randint(20, 30)
gets_num = random.randint(5, puts_num - 1)
pq = PriorityQueue()
print('PRIORITY QUEUE CURRENT STATE: ')
print(pq)
for _ in range(puts_num):
key, obj = key_type(), random_obj()
print()
print(f'PUT KEY={key}, OBJECT={obj}')
pq.put((key, obj))
print('PRIORITY QUEUE CURRENT STATE: ')
print(pq)
for _ in range(gets_num):
key, obj = pq.get()
print()
print(f'GET KEY={key}, OBJECT={obj}')
print('PRIORITY QUEUE CURRENT STATE: ')
print(pq)
def prim(agraph, start):
"""
Prim's algorithm for minimum spanning tree
Using min-heap data structure
return a minimum spanning tree
"""
# vertex of minimun spanning tree
mst_vertex = []
pq = PriorityQueue()
for v in agraph:
v.setDistance(sys.maxsize)
v.setPred(None)
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in agraph])
while not pq.isEmpty():
u = pq.delMin()
mst_vertex.append(u)
for adjacent in u.getConnections():
newcost = u.getWeight(adjacent)
if adjacent in pq and newcost < adjacent.getDistance():
adjacent.setPred(u)
adjacent.setDistance(newcost)
pq.decreaseKey(adjacent, newcost)
# edges of minimum spanning tree
mst = []
for i in range(1, len(mst_vertex)):
# u, v, cost
mst.append(
(mst_vertex[i - 1], mst_vertex[i], mst_vertex[i].getDistance()))
return mst
def __init__(self, world):
self.world = world
self.size = (len(world), len(world[0]))
# self.open = SortedList()
self.open = PriorityQueue()
self.openValue = 1
self.closedValue = 2
def dijkstra_search(root, goal, init, domprob, nb_robots, width, height):
"""Dijkstra search of a solution in a graph.
Returns a path if there is any.
The priority of each node represent the cost
(if each action costs 1) to go from the root
to the node.
Parameters:
root: Node object, the root of the state graph we're
searching and building at the same time.
goal: bitvector, the state we're searching.
init: bitvector, the initial state of the maze.
domprob: Domain-Problem object from the pddlpy lib.
nb_robots: integer, the number of robots in the maze.
width, height: integers, the dimensions of the maze.
"""
# Priority queue
pqueue = PriorityQueue()
# an empty set to maintain visited nodes
closed_set = set()
# a dictionary for path formation
meta = dict() # key -> (parent state, action to reach child)
# Operator list
op_list = list(domprob.operators())
# initialize
pqueue.insert(root)
meta[root] = (None, None)
ground_op_bv = get_ground_operator(
op_list, domprob, init, nb_robots, width, height)
print("Taille de ground_op : {}".format(len(ground_op_bv[0])))
while not pqueue.empty():
subtree_root = pqueue.dequeue()
current_priority = subtree_root.priority
if is_goal(subtree_root, goal):
return construct_path(subtree_root, meta)
# Create current node's children
for op in ground_op_bv:
subtree_root.build_children(op)
for (child, action) in subtree_root.children:
# The node has already been processed, so skip over it
if child in closed_set:
continue
# The child is not enqueued to be processed,
# so enqueue this level of children to be expanded
if child not in pqueue.queue:
child.priority = current_priority + 1
# Update the path
meta[child] = (subtree_root, action)
# Enqueue this node
pqueue.insert(child)
closed_set.add(subtree_root)
def get_surrounding_tiles(self, position, order='finish'):
""" Liefert eine Queue der angrenzenden Tiles zurück."""
tiles = PriorityQueue(order)
# print('Order: ', order)
# self.surroundings
for surround in self.surroundings:
# Ränder abfragen
# y unten
if position[0] == len(self.tiles) - 1 and surround[0] == +1:
continue
# y oben
if position[0] == 0 and surround[0] == -1:
continue
# x rechts
if position[1] == len(self.tiles[0]) - 1 and surround[1] == +1:
continue
# x links
if position[1] == 0 and surround[1] == -1:
continue
x = position[1] + surround[1]
y = position[0] + surround[0]
tiles.insert(self.tiles[y][x])
# Wenn Position am unteren Rande der y-Achse ist
# tiles.sort(key=lambda x: x.estimated_cost_to_finish, reverse=False)
return tiles
def test_push_pop(self):
p = PriorityQueue()
assert p.heap.items == []
p.enqueue('A', 2)
assert p.heap.items == [(2, 'A')]
p.enqueue('B', 2)
assert p.heap.items == [(2, 'A'), (2, 'B')]
p.enqueue('C', 1)
assert p.heap.items == [(1, 'C'), (2, 'B'), (2, 'A')]
p.enqueue('D', 1)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B')]
p.enqueue('E', 3)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B'),
(3, 'E')]
p.enqueue('F', 2)
assert p.heap.items == [(1, 'C'), (1, 'D'), (2, 'A'), (2, 'B'),
(3, 'E'), (2, 'F')]
assert p.push_pop('G', 1) == 'C'
assert p.heap.items == [(1, 'D'), (1, 'G'), (2, 'A'), (2, 'B'),
(3, 'E'), (2, 'F')]
assert p.push_pop('H', 2) == 'D'
assert p.heap.items == [(1, 'G'), (2, 'B'), (2, 'A'), (2, 'H'),
(3, 'E'), (2, 'F')]
assert p.push_pop('I', 3) == 'G'
assert p.heap.items == [(2, 'A'), (2, 'B'), (2, 'F'), (2, 'H'),
(3, 'E'), (3, 'I')]
assert p.push_pop('J', 4) == 'A'
assert p.heap.items == [(2, 'B'), (2, 'H'), (2, 'F'), (4, 'J'),
(3, 'E'), (3, 'I')]
assert p.push_pop('K', 1) == 'B'
assert p.heap.items == [(1, 'K'), (2, 'H'), (2, 'F'), (4, 'J'),
(3, 'E'), (3, 'I')]
assert p.push_pop('L', 3) == 'K'
assert p.heap.items == [(2, 'F'), (2, 'H'), (3, 'I'), (4, 'J'),
(3, 'E'), (3, 'L')]
def get_path(self, start, end, board, cost_estimate=get_distance):
t0 = time.time()
explored = set()
previous = {}
previous[start] = None
moves = {}
moves[start] = 0
frontier = PriorityQueue()
frontier.insert(start, cost_estimate(start, end))
if VERBOSE_ASTAR: print 'get_path start, end:', start, end
while not frontier.is_empty():
if (time.time() - t0 > PATHFINDER_TIMEOUT):
print 'PATHFINDING TIMEOUT: Averting disconnect...'
print ' get_path: Probably could not find a valid path from', start, 'to', end
return [start, start]
if VERBOSE_ASTAR: print 'get_path frontier:', frontier
current = frontier.remove()
explored.add(current)
if VERBOSE_ASTAR: print 'get_path explored set', explored
if VERBOSE_ASTAR: print 'get_path current pos:', current
if (current == end):
if VERBOSE_ASTAR: print 'Found end loc'
break
else:
neighbors = get_neighboring_locs(current, board)
if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors
for n in neighbors:
if n not in explored and (board.passable(n)
or n in (start, end)):
moves[n] = moves[current] + MOVE_COST
frontier.insert(n, cost_estimate(n, end) + moves[n])
previous[n] = current
# found goal, now reconstruct path
i = end
path = [i]
while i != start:
if (i in previous):
path.append(previous[i])
i = previous[i]
else:
print 'get_path error: probably could not find a valid path from', start, 'to', end
return [start, start] # return something valid
path.reverse()
return path
def test_init_with_list(self):
q = PriorityQueue()
q.enqueue('B', 3)
q.enqueue('C', 5)
q.enqueue('A', 1)
assert q.front() == 'A'
assert q.length() == 3
assert q.is_empty() is False
def print_ordered(self):
pq = PriorityQueue()
for n in self.d:
prob = self.d[n]
pq.add_task(n, prob)
#endfor
sorted = pq.sorted_queue()
norms = [x for x in reversed(sorted)]
for n in norms:
print n, self.d[n]
def test_is_empty(self):
p = PriorityQueue()
assert p.is_empty() is True
p.enqueue('A', 1)
assert p.is_empty() is False
p.enqueue('B', 1)
assert p.is_empty() is False
p.dequeue()
assert p.is_empty() is False
p.dequeue()
assert p.is_empty() is True
def test_dequeue(self):
pq = PriorityQueue([(1, 'A'), (2, 'B'), (3, 'C')])
assert pq.dequeue() == (1, 'A')
assert pq.length() == 2
assert pq.dequeue() == (2, 'B')
assert pq.length() == 1
assert pq.dequeue() == (3, 'C')
assert pq.length() == 0
assert pq.is_empty() is True
with self.assertRaises(ValueError):
pq.dequeue()
def test_front(self):
pq = PriorityQueue()
assert pq.front() is None
pq.enqueue('A', 1)
assert pq.front() == (1, 'A')
pq.enqueue('B', 2)
assert pq.front() == (1, 'A')
pq.dequeue()
assert pq.front() == (2, 'B')
pq.dequeue()
assert pq.front() is None
def test_length(self):
pq = PriorityQueue()
assert pq.length() == 0
pq.enqueue('A', 1)
assert pq.length() == 1
pq.enqueue('B', 2)
assert pq.length() == 2
assert pq.dequeue() == (1, 'A')
assert pq.length() == 1
assert pq.dequeue() == (2, 'B')
assert pq.length() == 0
def test_front(self):
q = PriorityQueue()
assert q.front() is None
q.enqueue('A', 6)
assert q.front() == 'A'
q.enqueue('B', 10)
assert q.front() == 'A'
q.dequeue()
assert q.front() == 'B'
q.dequeue()
assert q.front() is None
def UCS(
initial_state,
avoid_backtrack=False,
filtering=False,
cutoff=INF,
state_callback_fn=lambda state:
False, # A callback function for extended states. If it returns True, terminate
counter={
'num_enqueues': 0,
'num_extends': 0
}): # A counter for
frontier = PriorityQueue()
frontier.append(initial_state, initial_state.get_path_cost())
extended_filter = set()
while frontier: # frontier is False when it is empty. So just keep going until out of places to go...
# choose next state to "extend" from frontier
ext_node = frontier.pop()
if (filtering and ext_node.get_all_features() in extended_filter):
continue
extended_filter.add(ext_node.get_all_features())
counter['num_extends'] += 1
# are we there? If so, return the node.
if ext_node.is_goal_state():
return ext_node
# Update our caller (e.g. GUI) with the state we're extending.
# Terminate search early if True is returned.
if (state_callback_fn(ext_node)):
break
### Update frontier with next states
for state in ext_node.generate_next_states():
if (avoid_backtrack and ext_node.get_parent() == state):
continue
if (filtering and state.get_all_features() in extended_filter):
continue
if (cutoff != INF and state.get_path_length() > cutoff):
continue
frontier.append(state, state.get_path_cost())
counter['num_enqueues'] += 1
# if loop breaks before finding goal, search is failure; return None
return None
def test_length(self):
q = PriorityQueue()
assert q.length() == 0
q.enqueue('A', 5)
assert q.length() == 1
q.enqueue('B', 1)
assert q.length() == 2
q.dequeue()
assert q.length() == 1
q.dequeue()
assert q.length() == 0
def test_enqueue(self):
q = PriorityQueue()
q.enqueue('A', 3)
assert q.front() == 'A'
assert q.length() == 1
q.enqueue('B', 1)
assert q.front() == 'B'
assert q.length() == 2
q.enqueue('C', 4)
assert q.front() == 'B'
assert q.length() == 3
assert q.is_empty() is False
def test_enqueue(self):
pq = PriorityQueue()
pq.enqueue('B', 2)
assert pq.front() == (2, 'B')
assert pq.length() == 1
pq.enqueue('A', 1)
assert pq.front() == (1, 'A')
assert pq.length() == 2
pq.enqueue('C', 3)
assert pq.front() == (1, 'A')
assert pq.length() == 3
assert pq.is_empty() is False
def __init__(self, node_id):
self.node_id = node_id
self.timestamp = 0
self.nodeudp = NodeUDP(node_id)
self.thread = threading.Thread(target=self.listen, args=())
self.thread.setDaemon(True)
self.lock = threading.Lock()
self.thread.start()
self.q = PriorityQueue()
self.q_lock = threading.Lock()
self.replied_list = []
self.reply_lock = threading.Lock()
def dijkstra(self, label: str):
self._vertices[label].weight = 0
pq: PriorityQueue = PriorityQueue()
for label in self._vertices:
pq.insert(self._vertices[label])
while not pq.is_empty():
current: Vertex = pq.delete_min()
for neighbour in self._adjacency_map[current.label]:
v: Vertex = self._vertices[neighbour.label]
if current.weight + neighbour.weight < v.weight:
v.weight = current.weight + neighbour.weight
self._prev[v.label] = current.label
pq.decrease_key(v.key)
def dijkstra_pyds3(agraph, start):
pq = PriorityQueue()
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in agraph])
while not pq.isEmpty():
currentVert = pq.delMin()
for nextVert in currentVert.getConnections():
newDist = currentVert.getDistance() + currentVert.getWeight(
nextVert)
if newDist < nextVert.getDistance():
nextVert.setDistance(newDist)
nextVert.setPred(currentVert)
pq.decreaseKey(nextVert, newDist)
def test_front(self):
p = PriorityQueue()
assert p.front() is None
p.enqueue('A', 2)
assert p.front() == 'A'
p.enqueue('B', 2)
assert p.front() == 'A'
p.enqueue('C', 1)
assert p.front() == 'C'
p.dequeue()
assert p.front() == 'A'
p.dequeue()
assert p.front() == 'B'
def dijkstra(agraph, start):
pq = PriorityQueue()
start.setdistance(0)
pq.buildheap([(v.getdistance(), v) for v in agraph])
while not pq.isEmpty():
currentvert = pq.delmin()
for nextvert in currentvert.get_connections():
newdist = currentvert.getdistance() + currentvert.get_weight(
nextvert)
if newdist < nextvert.getdistance():
nextvert.setdistance(newdist)
nextvert.setpred(currentvert)
pq.decreasekey(newvert, newdist)
def __init__(self, position_start, position_finish, map):
tiles = []
self.open_list = PriorityQueue()
self.closed_list = PriorityQueue()
# queue = PriorityQueue()
self.map = map
self.finish_reached = False
self.route = []
self.position_start = position_start
self.position_finish = position_finish
self.open_list.insert(
TileInfo(position_start,
self.get_estimated_cost_to_finish(position_start), 0, -1,
-1))
for y in range(0, map.height):
tiles.append([])
for x in range(0, map.width):
tile = TileInfo((y, x), map.tiles[y][x],
self.get_estimated_cost_to_finish((y, x)),
99999, -1)
tiles[y].append(tile)
self.tiles = tiles
self.navi_active = False
self.recursion_level = 0
self.max_recursion_level = 100
self.use_diagonal_tiles = True
# Array für die Abfrage der umgebenden Tiles
self.surroundings = []
if self.use_diagonal_tiles == True:
self.surroundings.append((-1, -1))
self.surroundings.append((-1, +1))
self.surroundings.append((+1, -1))
self.surroundings.append((+1, +1))
self.surroundings.append((-1, 0))
self.surroundings.append((0, -1))
self.surroundings.append((0, +1))
self.surroundings.append((+1, 0))
def test_dequeue(self):
q = PriorityQueue()
q.enqueue('A', 1)
q.enqueue('B', 2)
q.enqueue('C', 3)
assert q.dequeue() == 'A'
assert q.length() == 2
assert q.dequeue() == 'B'
assert q.length() == 1
assert q.dequeue() == 'C'
assert q.length() == 0
assert q.is_empty() is True
with self.assertRaises(ValueError):
q.dequeue()
