Esempio n. 1
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)
Esempio n. 2
0
    def get_surrounding_tiles(self, position, order='finish'):
        """ Liefert eine Queue der angrenzenden Tiles zurück."""
        tiles = PriorityQueue(order)
        # print('Order: ', order)
        # self.surroundings

        for surround in self.surroundings:
            # Ränder abfragen
            # y unten
            if position[0] == len(self.tiles) - 1 and surround[0] == +1:
                continue
            # y oben
            if position[0] == 0 and surround[0] == -1:
                continue
            # x rechts
            if position[1] == len(self.tiles[0]) - 1 and surround[1] == +1:
                continue
            # x links
            if position[1] == 0 and surround[1] == -1:
                continue

            x = position[1] + surround[1]
            y = position[0] + surround[0]
            tiles.insert(self.tiles[y][x])

        # Wenn Position am unteren Rande der y-Achse ist

        # tiles.sort(key=lambda x: x.estimated_cost_to_finish, reverse=False)
        return tiles
Esempio n. 3
0
    def get_path(self, start, end, board, cost_estimate=get_distance):

        t0 = time.time()

        explored = set()
        previous = {}
        previous[start] = None
        moves = {}
        moves[start] = 0

        frontier = PriorityQueue()
        frontier.insert(start, cost_estimate(start, end))

        if VERBOSE_ASTAR: print 'get_path start, end:', start, end

        while not frontier.is_empty():

            if (time.time() - t0 > PATHFINDER_TIMEOUT):
                print 'PATHFINDING TIMEOUT: Averting disconnect...'
                print '    get_path: Probably could not find a valid path from', start, 'to', end
                return [start, start]

            if VERBOSE_ASTAR: print 'get_path frontier:', frontier

            current = frontier.remove()
            explored.add(current)

            if VERBOSE_ASTAR: print 'get_path explored set', explored
            if VERBOSE_ASTAR: print 'get_path current pos:', current

            if (current == end):
                if VERBOSE_ASTAR: print 'Found end loc'
                break
            else:
                neighbors = get_neighboring_locs(current, board)

                if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors

                for n in neighbors:
                    if n not in explored and (board.passable(n)
                                              or n in (start, end)):
                        moves[n] = moves[current] + MOVE_COST
                        frontier.insert(n, cost_estimate(n, end) + moves[n])
                        previous[n] = current

        # found goal, now reconstruct path
        i = end
        path = [i]
        while i != start:
            if (i in previous):
                path.append(previous[i])
                i = previous[i]
            else:
                print 'get_path error: probably could not find a valid path from', start, 'to', end
                return [start, start]  # return something valid

        path.reverse()

        return path
Esempio n. 4
0
    def get_path(self, start, end, board, cost_estimate=get_distance):
    
        t0 = time.time()

        explored = set()
        previous = {}
        previous[start] = None
        moves = {}
        moves[start] = 0

        frontier = PriorityQueue()
        frontier.insert(start, cost_estimate(start, end))

        if VERBOSE_ASTAR: print 'get_path start, end:', start, end

        while not frontier.is_empty():
        
            if (time.time() - t0 > PATHFINDER_TIMEOUT):
                print 'PATHFINDING TIMEOUT: Averting disconnect...'
                print '    get_path: Probably could not find a valid path from', start, 'to', end
                return [start, start] 

            if VERBOSE_ASTAR: print 'get_path frontier:', frontier

            current = frontier.remove()
            explored.add(current)

            if VERBOSE_ASTAR: print 'get_path explored set', explored
            if VERBOSE_ASTAR: print 'get_path current pos:', current

            if (current == end):
                if VERBOSE_ASTAR: print 'Found end loc'
                break
            else:
                neighbors = get_neighboring_locs(current, board)

                if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors

                for n in neighbors:
                    if n not in explored and (board.passable(n) or n in (start, end)):
                        moves[n] = moves[current] + MOVE_COST
                        frontier.insert(n, cost_estimate(n, end) + moves[n])
                        previous[n] = current

        # found goal, now reconstruct path
        i = end
        path = [i]
        while i != start:
            if (i in previous):
                path.append(previous[i])
                i = previous[i]
            else:
                print 'get_path error: probably could not find a valid path from', start, 'to', end
                return [start, start]    # return something valid

        path.reverse()

        return path
Esempio n. 5
0
    def choose_best(self, locs):
        best = None
        q = PriorityQueue()

        print 'choose best, locs:', locs

        if locs != None:
            for loc in locs:
                q.insert(loc, -self.score_loc(loc))  # by highest score
            best = q.remove()

        print 'choose best, best:', best

        return best
Esempio n. 6
0
def main():
    maze = Maze("maze2.png")
    paths = PriorityQueue()

    start = maze.getStart()
    end = maze.getEnd()
    paths.insert(PathHead(start[0], start[1], calcDistance(start, end)))
    current = paths.minimum()

    while paths.size() > 0 and current.getDistance() != 0.0:
        current = paths.extractMin()
        maze.setNodeVisited(current.getCords())
        surroundings = maze.checkSurroundings(current.getCords())
        insertSurroundings(paths, surroundings, current, end)

    solved = maze.getMaze()
    solved = numpy.array(solved)
    img = Image.fromarray(solved.astype('uint8'), 'RGB')
    img.save('solved.png')
Esempio n. 7
0
import priorityqueue
from priorityqueue import PriorityQueue
import numpy as np

pq = PriorityQueue(0, 20)
ints = np.random.randint(1, 100, size=20)
print("Inserting 20 integers into pq: {0}".format(ints))
[pq.insert(i) for i in ints]
print("pq is full: {0}".format(pq.isFull()))
print("pq size: {0}".format(pq.size()))
print("Deleting 20 integers from pq: {0}".format(
    [pq.delMin() for i in range(20)], sep=','))
print("pq is empty: {0}".format(pq.isEmpty()))
print("pq size: {0}".format(pq.size()))
Esempio n. 8
0
class Navi:
    def __init__(self, position_start, position_finish, map):
        tiles = []
        self.open_list = PriorityQueue()
        self.closed_list = PriorityQueue()
        # queue = PriorityQueue()
        self.map = map
        self.finish_reached = False
        self.route = []
        self.position_start = position_start
        self.position_finish = position_finish
        self.open_list.insert(
            TileInfo(position_start,
                     self.get_estimated_cost_to_finish(position_start), 0, -1,
                     -1))
        for y in range(0, map.height):
            tiles.append([])
            for x in range(0, map.width):
                tile = TileInfo((y, x), map.tiles[y][x],
                                self.get_estimated_cost_to_finish((y, x)),
                                99999, -1)
                tiles[y].append(tile)
        self.tiles = tiles
        self.navi_active = False
        self.recursion_level = 0
        self.max_recursion_level = 100
        self.use_diagonal_tiles = True

        # Array für die Abfrage der umgebenden Tiles
        self.surroundings = []
        if self.use_diagonal_tiles == True:
            self.surroundings.append((-1, -1))
            self.surroundings.append((-1, +1))
            self.surroundings.append((+1, -1))
            self.surroundings.append((+1, +1))
        self.surroundings.append((-1, 0))
        self.surroundings.append((0, -1))
        self.surroundings.append((0, +1))
        self.surroundings.append((+1, 0))

    def navi_step(self, tile_work='next'):
        # map = self.map
        # print('navistep')
        self.recursion_level += 1
        if tile_work == 'next':
            tile_work = self.open_list.get_and_delete()

        # pre_tile = self.tiles[tile_work.position[0]][tile_work.position[1]].pre_tile

        # Den Vorgänger-Tile des work-Tiles holen
        pre_tile = self.get_pre_tile(tile_work)
        # Wenn der Tile > -1 ist, hole die Kosten zum Start.
        if not pre_tile == -1:
            pre_tile_cost_from_start = self.tiles[pre_tile[0]][
                pre_tile[1]].cost_from_start
        else:
            pre_tile_cost_from_start = -1

        # Wenn der Work-Tile die Zielposition, also das Ziel erreicht ist.
        if tile_work.position == self.position_finish:
            self.map.add_status_text_with_clear("FINISH")
            tile_work.set_route_cost(pre_tile_cost_from_start + 1)
            self.route_finished(tile_work)
            self.finish_reached = True
        if pre_tile_cost_from_start >= 99999:
            pre_tile_cost_from_start = 0

        # Work-Tile: Die Kosten zum Start sind Pre-Tile + 1
        tile_work_cost_from_start = pre_tile_cost_from_start + 1
        tile_work.set_cost_from_start(tile_work_cost_from_start)
        tile_work.set_route_cost(
            self.get_estimated_cost_to_finish(tile_work.position) +
            tile_work.cost_from_start)
        tile_work.status = 0
        # Der Work-Tile wurde berechnet und kann also auf die Closed-List
        self.closed_list.insert(tile_work)
        self.tiles[tile_work.position[0]][
            tile_work.position[1]].type = "closed"

        # Um weiter zu machen, holen wir uns die umgebenden Tiles
        surrounding_tiles = self.get_surrounding_tiles(tile_work.position)

        # Solange wir noch nicht alle Tiles bearbeitet haben, durchlaufen wir die while-Schleife
        while not surrounding_tiles.isEmpty():
            # print(surrounding_tiles.get_size())
            surrounding_tile = surrounding_tiles.get_and_delete()

            if surrounding_tile == False:
                # print("Surround: no next tiles")
                break
            if surrounding_tile.type == "wall":
                # print('Surround: wall')
                continue

            tile_cost_from_start = tile_work_cost_from_start + 1

            if self.closed_list.exist(surrounding_tile):
                # Wenn ein Tile bereits in der closedlist ist, wurde er schon mal hinzugefügt
                # Es wird dann gecheckt, ob ...?
                # print('Surround: is in closedlist')
                continue
            elif self.open_list.exist(surrounding_tile):
                # Wenn ein Tile bereits in der openlist ist, wurde er schon mal hinzugefügt
                # Es wird dann gecheckt, ob ...?
                # print('Surround: is in openlist')
                tile_from_open_list = self.open_list.get_tile_and_delete(
                    surrounding_tile)
                # print(tile_from_open_list.cost_from_start, tile_cost_from_start)
                if tile_from_open_list.cost_from_start + 1 >= tile_cost_from_start:
                    # print('Surround: Neuer Weg ist teurer')
                    continue
                else:
                    # print('Surround: Neuer Weg ist günstiger')
                    tile_from_open_list.cost_from_start = surrounding_tile.cost_from_start + 1
                    tile_from_open_list.set_route_cost(
                        self.get_estimated_cost_to_finish(
                            tile_from_open_list.position) +
                        tile_work_cost_from_start)
                    self.open_list.insert(tile_from_open_list)
                    continue
            else:
                if surrounding_tile.position == tile_work.pre_tile:
                    # Wenn der umliegende Tile der vorherige vom tile_work ist, kann er ignoriert werden
                    continue
                # Wenn bis hierher nichts dagegen spricht, ist der Tile legitim, um ihn in nem navistep zu bearbeiten
                # pre-tile festlegen
                surrounding_tile.pre_tile = tile_work.position
                # Den pre-tile auch in der tiles.Liste festlegen
                self.tiles[surrounding_tile.position[0]][
                    surrounding_tile.position[1]].pre_tile = tile_work.position

                # In die open-list einfügen
                self.open_list.insert(surrounding_tile)

                # Entsprechenden Tile als open markieren
                self.tiles[surrounding_tile.position[0]][
                    surrounding_tile.position[1]].type = "open"

        # print("Open List: ", self.open_list.get_size())
        # print("Closed List: ", self.closed_list.get_size())
        # print(self.finish_reached)
        # if self.finish_reached == False and self.recursion_level < self.max_recursion_level:

        #     self.navi_step()
        self.recursion_level = 0
        return (tile_work.position, tile_work.route_cost)

        # self.navi_step(tile.position,position)

    def route_finished(self, tile):
        """ Route wurde gefunden! """
        route = []
        route.append(tile.position)
        next_tile = tile.pre_tile
        while True:
            route.append(next_tile)
            if len(route) > 1000:
                print('Finish: Route > 1000')
                break
            # print(next_tile)
            next_tile = self.tiles[next_tile[0]][next_tile[1]].pre_tile
            if next_tile == self.position_start:
                print('Finish: Start erreicht.')
                break
            if next_tile == -1:
                break

        for tile_position in route:
            self.tiles[tile_position[0]][tile_position[1]].type = "route"
        self.map.add_status_text("Kosten: " + str(tile.get_route_cost()))
        print("Kosten: ", tile.get_route_cost())
        self.map.add_status_text("Länge Route: " + str(len(route)))
        print("Länge Route: ", len(route))
        # print(route)
        self.navi_active = False
        self.position_start = tile.position

    def get_next_navi_tile(self, surrounding_tiles, position, last_position):
        """ Liefert den nächsten Navi-Tile zurück. Checkt, ob alle Bedingungen eingehalten werden."""
        # Bedingungen:
        # 1. Tiletype != wand
        # 2. Tiletype != navi
        # 3. Tiletype != last_position
        # 4. Tile ist in self.queue
        for tile in surrounding_tiles:
            if not tile:
                return False
            tile_type = self.map.get_tile_type(tile.position)
            print(tile.position, tile_type)
            if not tile_type == "wall" and not tile_type == "navi" and not tile.position == last_position:
                return tile

        print("Sackgasse?")
        return False
        # if tile_surround.position == self.position_finish:
        #     print("FINISH")
        #     print("Routenlänge: ",len(self.route))

    def get_estimated_cost_to_finish(self, position):
        """ Liefert die estimated cost an gegebener Position zurück."""
        distance_to_point = float(
            sqrt((position[0] - self.position_finish[0])**2 +
                 (position[1] - self.position_finish[1])**2))
        return distance_to_point

    def get_pre_tile(self, tile):
        """ Liefert den Vorgänger zurück """
        # print('get_pre_tile()')
        surrounding_tiles = self.get_surrounding_tiles(tile.position,
                                                       order='start')
        # print('surrounding_tiles: ', surrounding_tiles)
        pre_tile = surrounding_tiles.get_and_delete()
        # print('pre_tile: ', pre_tile)
        return pre_tile.position

    def get_surrounding_tiles(self, position, order='finish'):
        """ Liefert eine Queue der angrenzenden Tiles zurück."""
        tiles = PriorityQueue(order)
        # print('Order: ', order)
        # self.surroundings

        for surround in self.surroundings:
            # Ränder abfragen
            # y unten
            if position[0] == len(self.tiles) - 1 and surround[0] == +1:
                continue
            # y oben
            if position[0] == 0 and surround[0] == -1:
                continue
            # x rechts
            if position[1] == len(self.tiles[0]) - 1 and surround[1] == +1:
                continue
            # x links
            if position[1] == 0 and surround[1] == -1:
                continue

            x = position[1] + surround[1]
            y = position[0] + surround[0]
            tiles.insert(self.tiles[y][x])

        # Wenn Position am unteren Rande der y-Achse ist

        # tiles.sort(key=lambda x: x.estimated_cost_to_finish, reverse=False)
        return tiles

    def show_open_list(self):
        for item in self.open_list.queue:
            print(item.position, item.get_estimated_cost_to_finish())

    def get_open_list(self):
        return self.open_list.queue

    def get_closed_list(self):
        return self.closed_list.queue

    def show_closed_list(self):
        for item in self.closed_list.queue:
            print(item.position)

    def get_finish_tile(self):
        return self.tiles[self.position_finish[0]][self.position_finish[1]]
Esempio n. 9
0
item = QueueItem("A", 1)
assert (item is not None)
assert (hasattr(item, "content"))
assert (hasattr(item, "priority"))
assert (item.content == "A")
assert (item.priority == 1)

q = PriorityQueue()
assert (q is not None)
assert (hasattr(q, "items"))
assert (hasattr(q, "insert"))
assert (hasattr(q, "delete"))
assert (hasattr(q, "is_empty"))

# A,1 B,2 C,3 D,4 E,5, F,6, G,7
q.insert("C", 3)
assert (len(q.items) == 1)
assert (q.items[0].priority == 3)
assert (q.items[0].content == "C")
assert (str(q) == "(C,3)")

item = q.delete()
assert (item.priority == 3)
assert (item.content == "C")
assert (len(q.items) == 0)
assert (str(q) == "<<E>>")

q.insert("C", 3)
assert (len(q.items) == 1)
assert (q.items[0].priority == 3)
assert (q.items[0].content == "C")
Esempio n. 10
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
Esempio n. 11
0
import priorityqueue
from priorityqueue import PriorityQueue
import numpy as np

pq = PriorityQueue(0,20)
ints = np.random.randint(1,100, size=20)
print("Inserting 20 integers into pq: {0}".format(ints))
[pq.insert(i) for i in ints]
print("pq is full: {0}".format(pq.isFull()))
print("pq size: {0}".format(pq.size()))
print("Deleting 20 integers from pq: {0}".format([pq.delMin() for i in range(20)], sep=','))
print("pq is empty: {0}".format(pq.isEmpty()))
print("pq size: {0}".format(pq.size()))
Esempio n. 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
Esempio n. 13
0
def find(mapdata, width, height, start, end):
    """ mapdata is a one-dimensional list of values, start and end are vectors of size 2 """
    # WRITE THIS FUNCTION

    open = PriorityQueue()
    closed = []
    curTile = MapTile(start, None, None, None, None)
    print(width, height, start, end)

    while curTile.coords != end:
        if onMap(curTile.coords, width, height):
            n = north(curTile.coords)
            nter = terraintype(mapdata, width, height, n)
            ntile = MapTile(n, LAT_COST, mandistance(n, end), nter, curTile)
            if nter and (ntile not in closed):
                print(ntile)
                open.insert(ntile)

            s = south(curTile.coords)
            ster = terraintype(mapdata, width, height, s)
            stile = MapTile(s, LAT_COST, mandistance(s, end), ster, curTile)
            if ster and (stile not in closed):
                print(stile)
                open.insert(stile)

            e = east(curTile.coords)
            eter = terraintype(mapdata, width, height, e)
            etile = MapTile(e, LAT_COST, mandistance(e, end), eter, curTile)
            if eter and (etile not in closed):
                print(etile)
                open.insert(etile)

            w = west(curTile.coords)
            wter = terraintype(mapdata, width, height, w)
            wtile = MapTile(w, LAT_COST, mandistance(w, end), wter, curTile)
            if wter and (wtile not in closed):
                print(wtile)
                open.insert(wtile)

            nw = northwest(curTile.coords)
            nwter = terraintype(mapdata, width, height, nw)
            nwtile = MapTile(nw, DIAG_COST, mandistance(nw, end), nwter, curTile)
            if nwter and (nwtile not in closed):
                print(nwtile)
                open.insert(nwtile)

            ne = northeast(curTile.coords)
            neter = terraintype(mapdata, width, height, ne)
            netile =  MapTile(ne, DIAG_COST, mandistance(ne, end), neter, curTile)
            if neter and (netile not in closed):
                print(netile)
                open.insert(netile)

            sw = southwest(curTile.coords)
            swter = terraintype(mapdata, width, height, sw)
            swtile = MapTile(sw, DIAG_COST, mandistance(sw, end), swter, curTile)
            if swter and (swtile not in closed):
                print(swtile)
                open.insert(swtile)

            se = southeast(curTile.coords)
            seter = terraintype(mapdata, width, height, se)
            setile = MapTile(se, DIAG_COST, mandistance(se, end), seter, curTile)
            if seter and (setile not in closed):
                print(setile)
                open.insert(setile)

        closed.append(curTile)
        print(open)
        curTile = open.remove()

    path = []
    if curTile.coords == end:
        while curTile.parent is not None:
            path.append(curTile.parent)
            curTile = curTile.parent
        print(path)