Example #1
0
File: qsim.py Project: joewing/ms3
 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
Example #2
0
def aStar(start, goal, neighbor_func, distance_func, heuristic_func):
    """Returns a sequence of nodes that optmizes for the least cost
    from the start node to the goal.

    Let's describe the data that we pass to this function:

    start: the start of the search.
    goal: the goal of the search.
    neighbor_func: a function that, given a state, returns a list of
                   neighboring states.
    distance_func: a function that takes two nodes, and returns
                   the distance between them.
    heuristic_func: a function that takes two nodes, and returns
                    the heuristic distance between them.

    Each state mush be hashable --- each state must support the
    hash() function.
    """
    pqueue = PriorityQueue()
    g_costs = {start : 1}
    parents = {start : start}
    
    pqueue.push(heuristic_func(start, goal), start)
    while not pqueue.isEmpty():
        next_cost, next_node = pqueue.pop()
        g_costs[next_node] = g_costs[parents[next_node]] \
                             + distance_func(next_node, parents[next_node])
        if next_node == goal: break
        children = neighbor_func(next_node)
        for child in children:
            updateChild(goal, distance_func, heuristic_func,
                        child, next_node, parents, g_costs, pqueue)
    return getPathToGoal(start, goal, parents)
Example #3
0
class TestPriorityQueue(unittest.TestCase):
    def setUp(self):
        self.pq = PriorityQueue(":memory:")

    def test_empty(self):
        result = self.pq.is_empty()
        self.assertTrue(result)

    def test_order(self):
        self.assertEqual("hello", self.pq.push(10, "hello"))
        self.pq.push(1, "foo")
        self.pq.push(30, "bar")
        self.pq.push(5, "baz")

        self.assertEqual("bar", self.pq.pop())
        self.assertEqual("hello", self.pq.pop())
        self.assertEqual("baz", self.pq.pop())
        self.assertEqual("foo", self.pq.pop())
class TestPriorityQueue(unittest.TestCase):

    def setUp(self):
        self.pq = PriorityQueue(":memory:")

    def test_empty(self):
        result = self.pq.is_empty()
        self.assertTrue(result)

    def test_order(self):
        self.assertEqual("hello", self.pq.push(10, "hello"))
        self.pq.push(1, "foo")
        self.pq.push(30, "bar")
        self.pq.push(5, "baz")

        self.assertEqual("bar", self.pq.pop())
        self.assertEqual("hello", self.pq.pop())
        self.assertEqual("baz", self.pq.pop())
        self.assertEqual("foo", self.pq.pop())
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
Example #6
0
    def search(self, start):

        node = Node(self.problem.initial, action=self.problem.cur_dir)

        if self.problem.goal_test(node.state):
            return node

        frontier = PriorityQueue(self.f)

        frontier.append(node)

        while frontier:

            node = frontier.pop()

            finish = pygame.time.get_ticks()
            #se demorou 90 % do tempo
            if finish - start >= 0.9 * self.problem.time:
                frontier.append(node)
                return node

            if self.problem.goal_test(node.state):
                return node

            self.explored.add(node.state)

            for child in node.expand(self.problem):

                if child.path_cost == 1 and child.state in self.problem.get_adv_next_p_head(
                ):
                    continue

                if child.state not in self.explored and child not in frontier:

                    frontier.append(child)

                # caso o node expandido ja estiver na frontier
                # fica na frontier o que tiver menor função de avaliação
                elif child in frontier:
                    incumbent = frontier[child]
                    if self.f(child) < self.f(incumbent):
                        del frontier[incumbent]
        return None
Example #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
Example #8
0
def a_star_graph_search(map,start,goal):   
    
    """ Frontier and explored must be either a hash or tree for fast 
    membership testing
    
    In this implementation node doesn't need to be hashable because it is 
    not used in membership testing, a dic is used to associate keys and values.
    it may be better to create a Node class"""
    
    node = create_node(start, map, goal)
    if goal == start:
        node["path"].append(start)
        return node
    
    frontier = PriorityQueue()
    frontier.append(node)
    explored = set()
    
    while frontier:
        node = frontier.pop()
        state = node["state"]
        if goal == state:
            return node
        explored.add(state) 
        for action in map.roads[state]: 
            """child_node is not created here to not be called if in explored"""
            if action not in explored and action not in frontier:
                child_node = create_node(action, map, goal, node)
                frontier.append(child_node)
            elif action in frontier:
                child_node = create_node(action, map, goal, node)
                """frontier[child_node] = node with same state as child_node"""
                if child_node['f'] < frontier[child_node]['f']:
                    del frontier[frontier[child_node]]
                    frontier.append(child_node)            
    return None           
Example #9
0
    pointList.append(((8,12), None))
    pointList.append(((16,9), None))
    pointList.append(((3,5), None))
    pointList.append(((6,1), None))
    sites = PriorityQueue(pointList[:200])



    #Test
    counter = 1
    # Go through sites
#    while sites and counter < 20:
    while sites:
#        try:
        s = sites.pop()
        #Check if site event
        if s[1] == None:
            #print (s)
            handle_site(s[0])
        else:
#            node = s[1]
#            print("len_node: " + str(len(node)))
            handle_circle(s)
#        except KeyError as err:
#            print ("error")
#            print err
#            break

        #Test
        if s[1] is None:
Example #10
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()
Example #11
0
    #for a in range(0,200):
    #    pointList.append(((random.randint(0,1000), random.randint(0,1000)), None))

    pointList.append(((8,12), None))
    pointList.append(((16,9), None))
    pointList.append(((3,5), None))
    pointList.append(((6,1), None))
    sites = PriorityQueue(pointList[:200])

    #Test
    counter = 1
    # Go through sites
#    while sites and counter < 20:
    while sites:
#        try:
        s = sites.pop()
        #Check if site event
        if s[1] == None:
            #print (s)
            handle_site(s[0])
        else:
#            node = s[1]
#            print("len_node: " + str(len(node)))
            handle_circle(s)
#        except KeyError as err:
#            print ("error")
#            print err
#            break

        #Test
        if s[1] is None:
Example #12
0
class AStar():
    '''
    Properties:

    public:
    - world: 2D array of Nodes

    internal:
    - size: (width, height) tuple of world
    - open: Nodes queue to evaluate (heap-based priority queue)
    '''

    #----------------------------------------------------------------------
    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 initSearch(self, start, goal, obstacles):
        ''' first, check we can achieve the goal'''
        if goal.type in obstacles:
            return False

        ''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
        self.open.clear()
        self.openValue += 2
        self.closedValue += 2
        
        ''' then init search variables'''
        self.start = start
        self.goal = goal
        self.obstacles = obstacles
        self.start.cost = 0
        self.addToOpen(self.start)
        self.goal.parent = None
        return True

    #----------------------------------------------------------------------
    def search(self):
        while not self.openIsEmpty():
            current = self.popFromOpen()
            if current == self.goal:
                break
            self.removeFromOpen(current)
            self.addToClosed(current)

            ''' generator passes : look at the 8 neighbours around the current node from open'''
            for (di, dj) in [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]:
                neighbour = self.getNode(current.i + di, current.j + dj)
                if (not neighbour) or (neighbour.type in self.obstacles):
                    continue

                '''the cost to get to this node is the current cost plus the movement
                cost to reach this node. Note that the heuristic value is only used
                in the open list'''
                nextStepCost = current.cost + self.getNeighbourCost(current, neighbour)
                
                '''if the new cost we've determined for this node is lower than 
                it has been previously makes sure the node has not been
                determined that there might have been a better path to get to
                this node, so it needs to be re-evaluated'''
                
                if nextStepCost < neighbour.cost and (self.inOpenList(neighbour) or self.inClosedList(neighbour)):
                    self.invalidateState(neighbour)
                        
                '''if the node hasn't already been processed and discarded then
                step (i.e. to the open list)'''
                if (not self.inOpenList(neighbour)) and (not self.inClosedList(neighbour)):
                    neighbour.cost = nextStepCost
                    neighbour.heuristic = self.getHeuristicCost(neighbour, self.goal)
                    neighbour.parent = current
                    self.addToOpen(neighbour)

            ''' exit with None = path not yet found'''
            yield None

        '''since we've run out of search 
        there was no path. Just return'''
        if self.goal.parent is None:
            return
        
        '''At this point we've definitely found a path so we can uses the parent
        references of the nodes to find out way from the target location back
        to the start recording the nodes on the way.'''
        path = []
        goal = self.goal
        while goal is not self.start:
            path.insert(0, (goal.i, goal.j))
            goal = goal.parent
        
        ''' done, exit with path'''
        yield path

    #-----------------------------------------------------------------------------
    def getNode(self, i, j):
        if i >=0 and i < self.size[0] and j >= 0 and j < self.size[1]:
            return self.world[i][j]
        else:
            return None

    #----------------------------------------------------------------------
    def getNeighbourCost(self, n1, n2):
        return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
    
    #----------------------------------------------------------------------
    def getHeuristicCost(self, n1, n2):
        return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
    
    #----------------------------------------------------------------------
    def invalidateState(self, node):
        node.state = 0

    #----------------------------------------------------------------------
    def popFromOpen(self):
#        return self.open.first()
        return self.open.pop()

    #----------------------------------------------------------------------
    def addToOpen(self, node):
#        self.open.add(node)
        self.open.insert(node)
        node.state = self.openValue
        
    #----------------------------------------------------------------------
    def inOpenList(self, node):
        return node.state is self.openValue
   
    #----------------------------------------------------------------------
    def removeFromOpen(self, node):
#        self.open.remove(node)
        self.open.remove(node)
        node.state = 0

    #----------------------------------------------------------------------
    def openIsEmpty(self):
#        return not self.open.size()
        return self.open.isEmpty()
        
    #----------------------------------------------------------------------
    def addToClosed(self, node):
        node.state = self.closedValue
        
    #----------------------------------------------------------------------
    def inClosedList(self, node):
        return node.state is self.closedValue
Example #13
0
    def sweep_line_algorithm(self):
        self.current = Point()

        pointsPQ = PriorityQueue()
        tree = TreeSet()

        pointsPQ.pushAll([seg.p for seg in self.segments])
        pointsPQ.pushAll([seg.q for seg in self.segments])

        res = 0
        #print [str(x) for x in pointsPQ]

        while not pointsPQ.isEmpty():

            self.current.__update__(pointsPQ.pop())

            #print "Round", current

            if self.current.status == 'left':
                #print "Adding", self.current.segment
                low, high = tree.add_high_low(self.current.segment)

                low = tree.lower(self.current.segment)
                high = tree.higher(self.current.segment)
                #print "Actual:", self.current.segment
                #print "Low:", low, self.current.segment.intersect(low) if low else False
                #print "High:", high, self.current.segment.intersect(high) if high else False

                if low:
                    if self.current.segment.intersect(low):
                        a = self.current.segment.intersection_point(low)
                        #print "Adding a:", a, self.current.segment, low
                        pointsPQ.push(a)

                if high:
                    if self.current.segment.intersect(high):
                        a = self.current.segment.intersection_point(high)
                        #print "Adding 2:", a, self.current.segment, high
                        pointsPQ.push(a)

            elif self.current.status == "right":
                low = tree.lower(self.current.segment)
                high = tree.higher(self.current.segment)

                if low and high:
                    if low.intersect(high):
                        a = low.intersection_point(high)
                        #print "Adding 3:", a, low, high
                        pointsPQ.push(a)

                tree.remove(self.current.segment)
                #print "Removing", self.current.segment

            elif self.current.status == "int":
                # exchange the position in tree of the two segments intersecting in current
                s1, s2 = self.current.segment
                #print "Between, swapping:", str(s1), str(s2)

                tree.swap(s1, s2)

                #print "After swap:", s1, s2, s1 is tree.lower(s2), s2 is tree.lower(s1)
                #print "Modifying segments starts"
                old_s1 = s1.p.node
                old_s2 = s2.p.node

                s1.set_p_node(self.current.node)
                s2.set_p_node(self.current.node)

                #print "Tree after modification:", [str(x) for x in tree]

                # s1
                if s1 is tree.lower(s2):
                    #print "... s1, s2, ..."

                    low = tree.lower(s1)
                    #print "s1:", s1, "low:", low, s1.intersect(low) if low else False

                    if low is not None:
                        if s1.intersect(low):
                            pointsPQ.push(s1.intersection_point(low))

                    high = tree.higher(s2)
                    #print "s2:", s2, "high:", high, s2.intersect(high) if high else False

                    if high is not None:
                        if s2.intersect(high):
                            pointsPQ.push(s2.intersection_point(high))

                elif s2 is tree.lower(s1):
                    #print "... s2, s1, ..."

                    high = tree.higher(s1)
                    #print "s1:", s1, "high:", high, s1.intersect(high) if high else False

                    if high is not None:
                        if s1.intersect(high):
                            pointsPQ.push(s1.intersection_point(high))

                    low = tree.lower(s2)
                    #print "s2:", s2, "low:", low, s2.intersect(low) if low else False

                    if low is not None:
                        if s2.intersect(low):
                            pointsPQ.push(s2.intersection_point(low))

                else:
                    print "Error"  #raise SweepPlaneException("Intersection point error!")
                res += 1

                s1.set_p_node(old_s1)
                s2.set_p_node(old_s2)

            else:
                print "Error 2"  #raise SweepPlaneException("Node without status!")
            #print "Tree", [str(x) for x in tree]
            #print ""
        self.nodes = self.nodes[:self.original_n_nodes]
        return res
Example #14
0
class AStar():
    '''
    Properties:

    public:
    - world: 2D array of Nodes

    internal:
    - size: (width, height) tuple of world
    - open: Nodes queue to evaluate (heap-based priority queue)
    '''

    #----------------------------------------------------------------------
    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 initSearch(self, start, goal, obstacles):
        ''' first, check we can achieve the goal'''
        if goal.type in obstacles:
            return False
        ''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
        self.open.clear()
        self.openValue += 2
        self.closedValue += 2
        ''' then init search variables'''
        self.start = start
        self.goal = goal
        self.obstacles = obstacles
        self.start.cost = 0
        self.addToOpen(self.start)
        self.goal.parent = None
        return True

    #----------------------------------------------------------------------
    def search(self):
        while not self.openIsEmpty():
            current = self.popFromOpen()
            if current == self.goal:
                break
            self.removeFromOpen(current)
            self.addToClosed(current)
            ''' generator passes : look at the 8 neighbours around the current node from open'''
            for (di, dj) in [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1),
                             (1, -1), (1, 0), (1, 1)]:
                neighbour = self.getNode(current.i + di, current.j + dj)
                if (not neighbour) or (neighbour.type in self.obstacles):
                    continue
                '''the cost to get to this node is the current cost plus the movement
                cost to reach this node. Note that the heuristic value is only used
                in the open list'''
                nextStepCost = current.cost + self.getNeighbourCost(
                    current, neighbour)
                '''if the new cost we've determined for this node is lower than 
                it has been previously makes sure the node has not been
                determined that there might have been a better path to get to
                this node, so it needs to be re-evaluated'''

                if nextStepCost < neighbour.cost and (
                        self.inOpenList(neighbour)
                        or self.inClosedList(neighbour)):
                    self.invalidateState(neighbour)
                '''if the node hasn't already been processed and discarded then
                step (i.e. to the open list)'''
                if (not self.inOpenList(neighbour)) and (
                        not self.inClosedList(neighbour)):
                    neighbour.cost = nextStepCost
                    neighbour.heuristic = self.getHeuristicCost(
                        neighbour, self.goal)
                    neighbour.parent = current
                    self.addToOpen(neighbour)
            ''' exit with None = path not yet found'''
            yield None
        '''since we've run out of search 
        there was no path. Just return'''
        if self.goal.parent is None:
            return
        '''At this point we've definitely found a path so we can uses the parent
        references of the nodes to find out way from the target location back
        to the start recording the nodes on the way.'''
        path = []
        goal = self.goal
        while goal is not self.start:
            path.insert(0, (goal.i, goal.j))
            goal = goal.parent
        ''' done, exit with path'''
        yield path

    #-----------------------------------------------------------------------------
    def getNode(self, i, j):
        if i >= 0 and i < self.size[0] and j >= 0 and j < self.size[1]:
            return self.world[i][j]
        else:
            return None

    #----------------------------------------------------------------------
    def getNeighbourCost(self, n1, n2):
        return (abs(n2.i - n1.i) + abs(n2.j - n1.j))

    #----------------------------------------------------------------------
    def getHeuristicCost(self, n1, n2):
        return (abs(n2.i - n1.i) + abs(n2.j - n1.j))

    #----------------------------------------------------------------------
    def invalidateState(self, node):
        node.state = 0

    #----------------------------------------------------------------------
    def popFromOpen(self):
        #        return self.open.first()
        return self.open.pop()

    #----------------------------------------------------------------------
    def addToOpen(self, node):
        #        self.open.add(node)
        self.open.insert(node)
        node.state = self.openValue

    #----------------------------------------------------------------------
    def inOpenList(self, node):
        return node.state is self.openValue

    #----------------------------------------------------------------------
    def removeFromOpen(self, node):
        #        self.open.remove(node)
        self.open.remove(node)
        node.state = 0

    #----------------------------------------------------------------------
    def openIsEmpty(self):
        #        return not self.open.size()
        return self.open.isEmpty()

    #----------------------------------------------------------------------
    def addToClosed(self, node):
        node.state = self.closedValue

    #----------------------------------------------------------------------
    def inClosedList(self, node):
        return node.state is self.closedValue