Ejemplo n.º 1
0
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
Ejemplo n.º 2
0
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
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
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
Ejemplo n.º 6
0
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))
Ejemplo n.º 7
0
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()
Ejemplo n.º 8
0
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