Пример #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
Пример #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
Пример #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
Пример #4
0
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)
Пример #5
0
 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
Пример #6
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
Пример #7
0
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
Пример #8
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()
        # 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()
            '''