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 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 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 demo_basic():
qu = PriorityQueue()
books = [
Book(random_str(), [random_str(), random_str()],
random.randint(-40, 1920), random.randint(3, 9821), random_str())
for _ in range(20)
]
prices = [random.randint(3, 582) for _ in range(20)]
for pair in zip(books, prices):
qu.put(pair)
random_character = BookCharacter(['random character'], [])
while qu:
book, price = qu.get()
print(book)
random_character.add_book(book, CharacterRole(price % 3))
print(random_character)
print(repr(random_character))
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()
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.info("node {} requests to enter CS".format(self.node_id))
self.request()
while True:
time.sleep(0.1)
self.q_lock.acquire()
top_request_tuple = self.q.get()
# self.q.put(top_request_tuple)
self.q_lock.release()
self.reply_lock.acquire()
reply_num = len(self.replied_list)
self.reply_lock.release()
if self.node_id == top_request_tuple[1] and reply_num == (
node_num - 1):
break
logger.info("node {} enters CS at {}".format(
self.node_id, self.timestamp))
time.sleep(random.randint(1, 5))
self.release()
logger.info("node {} leaves CS at {}".format(
self.node_id, self.timestamp))
time.sleep(1)
time.sleep(3)
def listen(self):
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))
msg_type = msg.split(":")[1]
if msg_type == "request":
request_str = msg.split(":")[0]
self.reply(request_str, node_id)
request_tuple = str2tuple(request_str)
self.q_lock.acquire()
self.q.push(request_tuple)
self.q_lock.release()
elif msg_type == "reply":
self.reply_lock.acquire()
self.replied_list.append(node_id)
self.reply_lock.release()
elif msg_type == "release":
self.q_lock.acquire()
self.q.pop()
self.q_lock.release()
def astar(grid, start, goal):
'''Return a path found by A* alogirhm
and the number of steps it takes to find it.
arguments:
grid - A nested list with datatype int. 0 represents free space while 1 is obstacle.
e.g. a 3x3 2D map: [[0, 0, 0], [0, 1, 0], [0, 0, 0]]
start - The start node in the map. e.g. [0, 0]
goal - The goal node in the map. e.g. [2, 2]
return:
path - A nested list that represents coordinates of each step (including start and goal node),
with data type int. e.g. [[0, 0], [0, 1], [0, 2], [1, 2], [2, 2]]
steps - Number of steps it takes to find the final solution,
i.e. the number of nodes visited before finding a path (including start and goal node)
>>> from main import load_map
>>> grid, start, goal = load_map('test_map.csv')
>>> astar_path, astar_steps = astar(grid, start, goal)
It takes 7 steps to find a path using A*
>>> astar_path
[[0, 0], [1, 0], [2, 0], [3, 0], [3, 1]]
'''
debug_draw = False
path = []
steps = 0
found = False
map = map2d(grid)
frontier = PriorityQueue()
frontier.put(start, map.get_manhattan_distance(start, goal))
came_from = {}
came_from[tuple(start)] = {
'from': None,
'cost': 0
}
while not frontier.is_empty():
(curr_cost, current) = frontier.get()
frontier.remove()
if tuple(goal) in came_from.keys():
found = True
break
for neighbor in map.get_neighbors(current):
if neighbor is None or map.get_value(neighbor) == 1:
continue
neighbor_cost = curr_cost - map.get_manhattan_distance(current, goal) + \
map.get_manhattan_distance(current, neighbor) + \
map.get_manhattan_distance(neighbor, goal)
if tuple(neighbor) not in came_from or \
neighbor_cost < came_from.get(tuple(neighbor)).get('cost'):
frontier.put(neighbor, neighbor_cost)
came_from[tuple(neighbor)] = {
'from': current,
'cost': neighbor_cost
}
if debug_draw: map.draw_path(start = start, goal = goal, path = path, came_from = came_from)
# found = True
steps = len(came_from) - 1
curr_point = goal
while curr_point != start:
path.append(curr_point)
curr_point = came_from.get(tuple(curr_point)).get('from')
path.append(start)
path.reverse()
if found:
print(f"It takes {steps} steps to find a path using A*")
else:
print("No path found")
return path, steps