def aStarSearch(graph, start, goal, heuristic):
# initialize Priority Queue
frontier = PriorityQueue()
frontier.put(start, 0)
previous = {}
currentCost = {}
previous[start] = None
currentCost[start] = 0
counter = 1
space = 0
# while frontier is not empty
while not frontier.empty():
current = frontier.get()
if current == goal:
break
for next in graph.neighbors(current):
# determine A* cost
new_cost = currentCost[current] + graph.distanceToDistination(graph.getWeight(current), graph.getWeight(next))
# check if the cost has gone down since last time we visited to determine if location has already been visited
if next not in currentCost or new_cost < currentCost[next]:
currentCost[next] = new_cost
# determine which heuristic to use
if heuristic == 1:
heuristicValue = heuristicEuclidean(graph.getWeight(current), graph.getWeight(goal))
else:
heuristicValue = heuristicChebyshev(graph.getWeight(current), graph.getWeight(goal))
# add heuristic cose to A* cost
priority = new_cost + heuristicValue
# add path with it's priority
frontier.put(next, priority)
previous[next] = current
counter = counter + 1
space = max(space, frontier.size())
return previous, currentCost, counter, space
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 greedyFirstSearch(graph, start, goal, heuristic):
# initialize priority queue
frontier = PriorityQueue()
frontier.put(start, 0)
previous = {}
previous[start] = None
counter = 1
space = 0
# if frontier isn't empty
while not frontier.empty():
current = frontier.get()
# check if current is the goal
if current == goal:
break
for next in graph.neighbors(current):
if next not in previous:
# Greedy Best First Search will only use the heuristic to determine the path to choose
if heuristic == 1:
heuristicValue = heuristicEuclidean(graph.getWeight(current), graph.getWeight(goal))
else:
heuristicValue = heuristicChebyshev(graph.getWeight(current), graph.getWeight(goal))
priority = heuristicValue
frontier.put(next, priority)
counter = counter + 1
previous[next] = current
space = max(space, frontier.size())
return previous, counter, space
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 run(self):
pq = PriorityQueue()
for q in self.queues:
t = q.reset()
pq.push(t, q)
while not pq.empty():
t = max(t, pq.key())
q = pq.value()
pq.pop()
next_t = q.process(t)
if next_t >= 0:
pq.push(next_t, q)
return t
def dijkstras(start):
queue = PriorityQueue()
queue.put(start, 0)
visited = []
distance = {start: 0}
previous = {start: None}
inf = float('inf')
while not queue.empty():
u = queue.get()
visited.append(u)
for v in u.neighbors:
if v not in visited:
tempDistance = distance.get(u, inf) + u.getWeight(v)
if tempDistance < distance.get(v, inf):
distance[v] = tempDistance
queue.put(v, tempDistance)
previous[v] = u
return distance
def a_search(graph, start, goals):
my_heap = PriorityQueue(
) #not really a heap just easier to call it that than queue
my_heap.push(start, 0)
pathway = []
cost_so_far = {}
came_from = {}
cost_so_far[start] = 0
came_from[start] = None
the_sum = 0
ordered_goals = []
while not my_heap.empty():
current = my_heap.pop()
#print("here")
if current in goals:
ordered_goals.append(current)
goals.remove(current)
for i in graph:
if i.parent:
the_sum += 1
while current.parent:
pathway.append(current)
x = current.parent
current.parent = None
current = x
if len(goals) == 0:
break
for i in current.edges:
new_score = current.gscore + 1 #increment g(n)
if i not in cost_so_far or new_score < i.gscore:
cost_so_far[i] = new_score
i.gscore = new_score
i.hscore = findMST(i, goals)
#print(i.hscore, current)
i.fscore = i.gscore + i.hscore
my_heap.push(i, i.fscore)
if not i.parent:
i.parent = current
return pathway, the_sum, ordered_goals
class Node():
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()
# self.pending_req = PriorityQueue()
# self.pending_locl = threading.Lock()
def send_to(self, message, node_id):
self.lock.acquire()
self.timestamp += 1
message = "<{},{}>:".format(self.timestamp, self.node_id) + message
logger.debug("node {} sends '{}' to node {} at {}".format(
self.node_id, message, node_id, self.timestamp))
self.lock.release()
self.nodeudp.send(message, ("127.0.0.1", startPort + node_id))
def broadcast(self, message):
global node_num
self.lock.acquire()
self.timestamp += 1
request_str = "<{},{}>".format(self.timestamp, self.node_id)
message = "<{},{}>:".format(self.timestamp, self.node_id) + message
logger.debug("node {} broadcasts '{}' at {}".format(
self.node_id, message, self.timestamp))
self.lock.release()
for node_id in range(node_num):
if node_id == self.node_id:
continue
self.nodeudp.send(message, ("127.0.0.1", startPort + node_id))
return request_str
def request(self):
request_str = self.broadcast("request")
request_tuple = str2tuple(request_str)
self.q_lock.acquire()
self.q.push(request_tuple)
self.q_lock.release()
def reply(self, request_str, to):
self.send_to("reply:" + request_str, to)
def release(self):
self.broadcast("release")
self.reply_lock.acquire()
self.replied_list = []
self.reply_lock.release()
self.q_lock.acquire()
self.q.pop()
self.q_lock.release()
def run(self):
global enter_times
global current
global node_num
for _ in range(enter_times):
logger.debug("node {} requests to enter CS".format(self.node_id))
self.request()
enter_flag = False
while True:
time.sleep(0.1)
if current == self.node_id:
enter_flag = True
if enter_flag and current != self.node_id:
break
time.sleep(1)
time.sleep(3)
def listen(self):
global current
while True:
msg = self.nodeudp.recv()[0]
ts = int(msg.split(',')[0][1:])
node_id = int(msg.split(',')[1].split('>')[0])
if ts > self.timestamp:
self.lock.acquire()
self.timestamp = ts + 1
self.lock.release()
else:
self.lock.acquire()
self.timestamp += 1
self.lock.release()
logger.debug("node {} recieve '{}' from node {} at {}".format(
self.node_id, msg, node_id, self.timestamp))
logger.debug("node {} queue: {}".format(self.node_id,
self.q.queue))
msg_type = msg.split(":")[1]
if msg_type == "request":
request_str = msg.split(":")[0]
request_tuple = str2tuple(request_str)
self.q_lock.acquire()
self.q.push(request_tuple)
top_req = self.q.get()
if top_req == request_tuple:
self.q.pop()
self.reply(request_str, node_id)
self.q_lock.release()
elif msg_type == "reply":
self.reply_lock.acquire()
self.replied_list.append(node_id)
reply_num = len(self.replied_list)
if reply_num == node_num - 1:
logger.info("node {} enters CS at {}".format(
self.node_id, self.timestamp))
current = self.node_id
time.sleep(random.randint(1, 5))
self.q_lock.acquire()
self.q.pop() # pop himself
while not self.q.empty():
pending_request_tuple = self.q.pop()
pending_request_str = tuple2str(pending_request_tuple)
self.reply(pending_request_str,
pending_request_tuple[1])
self.q_lock.release()
self.lock.acquire()
logger.info("node {} leaves CS at {}".format(
self.node_id, self.timestamp))
self.lock.release()
current = -1
self.replied_list = []
self.reply_lock.release()
'''