Ejemplo n.º 1
0
  def setUp(self):
    self.similarity = Similarity(1.0, 'dummy', None, None)

    t0_src_path = (0,)
    t0_trg_path = (10,)
    t0_src_subpaths = ((0, 0), (0, 1))
    t0_trg_subpaths = ((10, 0), (10, 1))
    self.t0 = Transformation(t0_src_path, t0_trg_path,
                             t0_src_subpaths, t0_trg_subpaths, self.similarity)

    t1_src_path = (1,)
    t1_trg_path = (11,)
    t1_src_subpaths = ((1, 0), (1, 1))
    t1_trg_subpaths = ((11, 0), (11, 1))
    self.t1 = Transformation(t1_src_path, t1_trg_path,
                             t1_src_subpaths, t1_trg_subpaths, self.similarity)

    t2_src_path = (2,)
    t2_trg_path = (12,)
    t2_src_subpaths = ((2, 0), (2, 1))
    t2_trg_subpaths = ((12, 0), (12, 1))
    self.t2 = Transformation(t2_src_path, t2_trg_path,
                             t2_src_subpaths, t2_trg_subpaths, self.similarity)

    t0bis_src_path = (0,)
    t0bis_trg_path = (10,)
    t0bis_src_subpaths = ((3, 0), (3, 1))
    t0bis_trg_subpaths = ((13, 0), (13, 1))
    self.t0bis = Transformation(t0bis_src_path, t0bis_trg_path,
                                t0bis_src_subpaths, t0bis_trg_subpaths, self.similarity)

    self.q_costs = PriorityQueue(2)
    self.q_probs = PriorityQueue(2, reverse=True)
Ejemplo n.º 2
0
    def __init__(self, simulation):
        '''
        Constructor for Waiting

        :param simulation: Simulation - reference to simulation object
        '''
        self.homes = PriorityQueue(key=lambda x: x[0])
        self.simulation = simulation
        self.fixed = False
        self.pathfinder = PathFinder(simulation.ground, cost_function)
Ejemplo n.º 3
0
def best_first_search(grid, start, end, heuristic_cost=manhattan_cost):
    closed_set = set()
    open_set = PriorityQueue()

    open_set.put(start, (heuristic_cost(start, end), grid_cost(start, grid)))

    prev = {}

    while not open_set.empty():
        curr = open_set.get()

        if is_goal(curr, end):
            return _reconstruct_path(curr, prev)

        for neighbour in _get_neighbours(curr, grid):
            if neighbour in closed_set:
                continue

            elif _is_obstacle(neighbour, grid):
                continue

            if neighbour not in open_set:
                open_set.put(neighbour, (heuristic_cost(
                    neighbour, end), grid_cost(neighbour, grid)))

            prev[neighbour] = curr
        closed_set.add(curr)

    return []
Ejemplo n.º 4
0
def dijkstras(graph):
    # Some very large int as stand in for inf
    dist = [999999999 for _ in range(len(graph))]
    dist[0] = 0
    prev = [None for _ in range(len(graph))]
    queue = PriorityQueue()
    for node, d in enumerate(dist):
        queue.add_with_priority(node, d)

    target = len(graph) - 1

    while True:
        # Find node with lowest dist in queue
        next_node = queue.pop()

        # Check if reached target node
        if next_node == target:
            break

        # Iterate over neighbours to queue
        for neighbour, risk in graph[next_node]:
            alt_dist = dist[next_node] + risk
            if dist[neighbour] is None or alt_dist < dist[neighbour]:
                dist[neighbour] = alt_dist
                prev[neighbour] = next_node
                queue.change_priority(neighbour, alt_dist)

    return dist[-1]
Ejemplo n.º 5
0
 def __init__(self, validator_set, protocol=BlockchainProtocol):
     self.validator_set = validator_set
     self.global_view = protocol.View(self._collect_initial_messages(),
                                      protocol.initial_message(None))
     self.message_queues = {
         validator: PriorityQueue()
         for validator in self.validator_set
     }
     self._current_time = 0
Ejemplo n.º 6
0
def Dijkstra(Graph, start, end):
    '''
    param:
    Graph: nx.Digraph()
    start: node which the path starts from
    end: node which the path ends in

    return:
    min_distance
    '''

    # 获得点的列表
    node_list = list(Graph.nodes())

    # 初始情况:起始点到其他所有点的距离未知,设为无穷大
    for node in node_list:
        if node != start:
            Graph.nodes[node]['min_dis'] = np.inf

    node_queue = PriorityQueue('min')
    node_queue.push(start, 0)  # 构造接下来要遍历的优先级队列

    while not node_queue.empty():
        current_node, _min_distance = node_queue.pop()  # 队列中弹出下一个遍历的点
        # 当我们已经找到一个到达当前节点的更优路径时,就不需要再在这个点搜索了
        if _min_distance > Graph.nodes[current_node]['min_dis']:
            continue

        for successor in Graph.neighbors(current_node):
            arc = (current_node, successor)
            _distance = Graph.nodes[current_node]['min_dis'] \
                        + Graph.edges[arc]['length'] # 计算起始点通过当前点到达后继节点的距离

            # 更新起始点到达后继节点的距离
            if _distance < Graph.nodes[successor]['min_dis']:
                Graph.nodes[successor]['min_dis'] = _distance
                node_queue.push(successor, _distance)  # 在队列中加入需要后继节点
                Graph.nodes[successor]['prev'] = current_node
        # 早停 必须和优先级队列一同使用
        if current_node == end:
            break

    min_distance = Graph.nodes[end]['min_dis']
    path = []
    prev_node = end
    while prev_node:
        path.insert(0, prev_node)
        prev_node = Graph.nodes[prev_node]['prev']

    return min_distance, path
Ejemplo n.º 7
0
def test_priority_queue(arr):
    output = sorted(arr)
    pq = PriorityQueue()

    for i in arr:
        pq.put(i, i)

    check = []
    for j in pq.len():
        check.append(pq.get())

    print(check, output)
    assert (check == output)
Ejemplo n.º 8
0
  def test_FilterPreservesHeapStructure(self):
    big_q = PriorityQueue(3)
    big_q.min_threshold = 5

    big_q.Push(5.0, self.t1)
    big_q.Push(0.1, self.t0)
    big_q.Push(0.2, self.t2)

    self.assertEqual(3, len(big_q.queue))
    
    big_q.FilterCache()

    self.assertEqual(2, len(big_q.queue))

    self.assertIn(self.t0, big_q.GetItems())
    self.assertIn(self.t2, big_q.GetItems())
    self.assertEqual(max(big_q.queue, key=lambda x: abs(x[0])), big_q.queue[0])
Ejemplo n.º 9
0
def a_star(grid, start, goal):
    # Initialize an open and closed set.
    # The open queue is a priority queue
    open_queue = PriorityQueue()
    open_queue.put(start, 0)
    closed = set()

    # g_score and f_score are dictionaries with infinity default values
    came_from = {}
    g_score = defaultdict(lambda: float('inf'))
    g_score[start] = 0

    f_score = defaultdict(lambda: float('inf'))
    f_score[start] = g_score[start] + heuristic(start, goal)

    while not open_queue.empty():
        # Pop the node with the lowest f score
        current = open_queue.get()
        closed.add(current)

        # If we hit our goal, return the path
        if current == goal:
            return reconstruct_path(came_from, start, goal)

        for neighbor in grid.neighbors(current):
            if neighbor in closed:
                continue

            # Calculate g score, heursitic and f score for the neighbor
            g = g_score[current] + grid.dist_between(current, neighbor)
            h = heuristic(neighbor, goal)
            f = g + h

            if neighbor not in open_queue:
                open_queue.put(neighbor, f)
            # If the g score is bigger than what we already have, skip it.
            elif g >= g_score[neighbor]:
                continue

            # update came from, g and f score
            came_from[neighbor] = current
            g_score[neighbor] = g
            f_score[neighbor] = f

    # Found no path
    return None
Ejemplo n.º 10
0
def test_priority_queue():
    arr = list(range(50))
    output = arr.copy()
    random.shuffle(arr)
    pq = PriorityQueue()

    for i in arr:
        pq.put(i, i)

    check = []
    for j in range(pq.len()):
        check.append(pq.get())

    assert check == output
Ejemplo n.º 11
0
    def bfs(self, depth_limit=-1, n_sols=1):
        """ Best first search with depth limit

        Performs discrete best first search, the result is saved in self.goals

        Args:
            depth_limit : int depth limit, if negative, no limit
            n_sols: int desired number of solution, if negative, find all
        
        Returns:
            nodes_visited: (integer) number of nodes visited
        """
        frontier = PriorityQueue()
        frontier.push(self.root)
        nodes_visited = 0
        goals = []

        while len(frontier):
            cur = frontier.pop()
            nodes_visited += 1
            print("Visiting Node", nodes_visited)
            print("Depth", cur.depth)

            if cur.parent is not None:
                print(cur.action)
                cur.calc_traj()
                print(cur.get_cost())

            if cur.g < 1e16:
                if cur.goal_reached():
                    goals.append(cur)
                    print("Found Goal!", len(goals))
                    # if desired number of goals reached, end loop
                    if len(goals) >= n_sols >= 0:
                        break

                if not (cur.depth >= depth_limit >= 0):
                    cur.expand()
                    for ch in cur.children:
                        frontier.push(ch)

        return (goals, nodes_visited)
Ejemplo n.º 12
0
def A_STAR(initial_state,heuristic):
    """A * search"""
    frontier = PriorityQueue('min',heuristic)
    frontier.append(initial_state)
    frontier_config = {}
    frontier_config[tuple(initial_state.config)] = True
    explored = set()
    nodes_expanded = 0
    max_search_depth = 0

    while frontier:
        state = frontier.pop()
        print("****** State ******")
        state.display()
        explored.add(state)
        if state.is_goal():
            return (state,nodes_expanded,max_search_depth)
        
        nodes_expanded += 1
        for neigbhor in state.expand(RLDU= False):
            if neigbhor not in explored and tuple(neigbhor.config) not in frontier_config:
                frontier.append(neigbhor)
                frontier_config[tuple(neigbhor.config)] = True
                if neigbhor.cost > max_search_depth:
                    max_search_depth = neigbhor.cost
            elif neigbhor in frontier:
                if heuristic(neigbhor) < frontier[neigbhor]:
                    frontier.__delitem__(neigbhor)
                    frontier.append(neigbhor)
    return None
Ejemplo n.º 13
0
class PriorityQueueTestCase(unittest.TestCase):
  def setUp(self):
    self.similarity = Similarity(1.0, 'dummy', None, None)

    t0_src_path = (0,)
    t0_trg_path = (10,)
    t0_src_subpaths = ((0, 0), (0, 1))
    t0_trg_subpaths = ((10, 0), (10, 1))
    self.t0 = Transformation(t0_src_path, t0_trg_path,
                             t0_src_subpaths, t0_trg_subpaths, self.similarity)

    t1_src_path = (1,)
    t1_trg_path = (11,)
    t1_src_subpaths = ((1, 0), (1, 1))
    t1_trg_subpaths = ((11, 0), (11, 1))
    self.t1 = Transformation(t1_src_path, t1_trg_path,
                             t1_src_subpaths, t1_trg_subpaths, self.similarity)

    t2_src_path = (2,)
    t2_trg_path = (12,)
    t2_src_subpaths = ((2, 0), (2, 1))
    t2_trg_subpaths = ((12, 0), (12, 1))
    self.t2 = Transformation(t2_src_path, t2_trg_path,
                             t2_src_subpaths, t2_trg_subpaths, self.similarity)

    t0bis_src_path = (0,)
    t0bis_trg_path = (10,)
    t0bis_src_subpaths = ((3, 0), (3, 1))
    t0bis_trg_subpaths = ((13, 0), (13, 1))
    self.t0bis = Transformation(t0bis_src_path, t0bis_trg_path,
                                t0bis_src_subpaths, t0bis_trg_subpaths, self.similarity)

    self.q_costs = PriorityQueue(2)
    self.q_probs = PriorityQueue(2, reverse=True)

  def test_RegularInsertions(self):
    self.q_costs.Push(0.3, self.t0)
    self.q_costs.Push(0.2, self.t1)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t0, self.q_costs.GetItems())
    self.assertIn(self.t1, self.q_costs.GetItems())

    self.q_probs.Push(0.3, self.t0)
    self.q_probs.Push(0.2, self.t1)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t0, self.q_probs.GetItems())
    self.assertIn(self.t1, self.q_probs.GetItems())

  def test_InsertionsOverflow(self):
    self.q_costs.Push(0.2, self.t0)
    self.q_costs.Push(0.3, self.t1)
    self.q_costs.Push(0.1, self.t2)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t0, self.q_costs.GetItems())
    self.assertIn(self.t2, self.q_costs.GetItems())
    self.assertNotIn(self.t1, self.q_costs.GetItems())

    self.q_probs.Push(0.2, self.t0)
    self.q_probs.Push(0.3, self.t1)
    self.q_probs.Push(0.1, self.t2)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t0, self.q_probs.GetItems())
    self.assertIn(self.t1, self.q_probs.GetItems())
    self.assertNotIn(self.t2, self.q_probs.GetItems())

  def test_InsertionsSameElementOverflow(self):
    self.q_costs.Push(0.1, self.t0)
    self.q_costs.Push(0.2, self.t1)
    self.q_costs.Push(0.1, self.t0)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t0, self.q_costs.GetItems())
    self.assertIn(self.t1, self.q_costs.GetItems())

    self.q_probs.Push(0.1, self.t0)
    self.q_probs.Push(0.2, self.t1)
    self.q_probs.Push(0.1, self.t0)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t0, self.q_probs.GetItems())
    self.assertIn(self.t1, self.q_probs.GetItems())

    self.q_probs.Push(0.2, self.t0)
    self.q_probs.Push(0.1, self.t1)
    self.q_probs.Push(0.2, self.t0)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t0, self.q_probs.GetItems())
    self.assertIn(self.t1, self.q_probs.GetItems())

  def test_InsertionsCostReplacement(self):
    self.q_costs.Push(0.3, self.t0)
    self.q_costs.Push(0.2, self.t1)
    self.q_costs.Push(0.1, self.t0)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t1, self.q_costs.GetItems())
    self.assertIn(self.t0, self.q_costs.GetItems())
    self.assertEqual(0.1, self.q_costs.GetBestScore())
    self.assertEqual(self.t0, self.q_costs.GetBestScoreItem())

    self.q_probs.Push(0.1, self.t0)
    self.q_probs.Push(0.2, self.t1)
    self.q_probs.Push(0.3, self.t0)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t0, self.q_probs.GetItems())
    self.assertIn(self.t1, self.q_probs.GetItems())
    self.assertEqual(0.3, self.q_probs.GetBestScore())
    self.assertEqual(self.t0, self.q_probs.GetBestScoreItem())

  """
  def test_InsertionsCostReplacementSimilarItem(self):
    self.q_costs.Push(0.3, self.t0)
    self.q_costs.Push(0.2, self.t1)
    self.q_costs.Push(0.1, self.t0bis)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t1, self.q_costs.GetItems())
    self.assertNotEqual(self.t0.src_subpaths,
                        self.q_costs.GetBestScoreItem().src_subpaths)
    self.assertEqual(self.t0bis.src_subpaths,
                     self.q_costs.GetBestScoreItem().src_subpaths)

    self.q_probs.Push(0.3, self.t0)
    self.q_probs.Push(0.2, self.t1)
    self.q_probs.Push(0.4, self.t0bis)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t1, self.q_probs.GetItems())
    self.assertNotEqual(self.t0.src_subpaths,
                        self.q_probs.GetBestScoreItem().src_subpaths)
    self.assertEqual(self.t0bis.src_subpaths,
                     self.q_probs.GetBestScoreItem().src_subpaths)
  """

  def test_InsertionsCostNotReplacementSimilarItem(self):
    self.q_costs.Push(0.1, self.t0)
    self.q_costs.Push(0.2, self.t1)
    self.q_costs.Push(0.4, self.t0bis)
    self.assertEqual(2, len(self.q_costs.queue))
    self.assertIn(self.t1, self.q_costs.GetItems())
    self.assertEqual(self.t0.src_subpaths,
                     self.q_costs.GetBestScoreItem().src_subpaths)
    self.assertNotEqual(self.t0bis.src_subpaths,
                        self.q_costs.GetBestScoreItem().src_subpaths)

    self.q_probs.Push(0.2, self.t0)
    self.q_probs.Push(0.1, self.t1)
    self.q_probs.Push(0.0, self.t0bis)
    self.assertEqual(2, len(self.q_probs.queue))
    self.assertIn(self.t1, self.q_probs.GetItems())
    self.assertEqual(self.t0.src_subpaths,
                     self.q_probs.GetBestScoreItem().src_subpaths)
    self.assertNotEqual(self.t0bis.src_subpaths,
                        self.q_probs.GetBestScoreItem().src_subpaths)

  def test_FilterPreservesHeapStructure(self):
    big_q = PriorityQueue(3)
    big_q.min_threshold = 5

    big_q.Push(5.0, self.t1)
    big_q.Push(0.1, self.t0)
    big_q.Push(0.2, self.t2)

    self.assertEqual(3, len(big_q.queue))
    
    big_q.FilterCache()

    self.assertEqual(2, len(big_q.queue))

    self.assertIn(self.t0, big_q.GetItems())
    self.assertIn(self.t2, big_q.GetItems())
    self.assertEqual(max(big_q.queue, key=lambda x: abs(x[0])), big_q.queue[0])
Ejemplo n.º 14
0
def a_star_search(grid, start, end, heuristic_cost=manhattan_cost):
    """
    Implementation of A Star over a 2D grid. Returns a list of waypoints
    as a list of (x,y) tuples.

    Input:
    : grid, 2D matrix
    : start, (x,y) tuple, start position
    : end, (x,y) tuple, end destination

    Output:
    : waypoints, list of (x,y) tuples
    """
    # the set of cells already evaluated
    closed_set = set()

    # the set of cells already discovered
    open_set = PriorityQueue()
    open_set.put(start, (heuristic_cost(start, end), grid_cost(start, grid)))

    # for each cell, mapping to its least-cost incoming cell
    prev = {}

    # for each node, cost of reaching it from start (g_cost)
    # for each node, cost of getting from start to dest via that node (f_cost)
    #   note: cell->dest component of f_cost will be estimated using a heuristic
    g_cost = {}
    for r in range(len(grid)):
        for c in range(len(grid[0])):
            cell = (r, c)
            g_cost[cell] = inf

    g_cost[start] = 0
    while not open_set.empty():
        # node in open set with min fscore
        curr = open_set.get()

        # if we've reached the destination
        if curr == end:
            return _reconstruct_path_to_destination(prev, curr)

        for neighbor in get_neighbours(curr, grid):
            # ignore neighbors which have already been evaluated
            if neighbor in closed_set:
                continue

            curr_g_score = g_cost[curr] + _grid_cost(neighbour, grid)
            # add neighbor to newly discovered nodes
            if neighbor not in open_set:
                f_cost = g_cost[neighbor] + heuristic_cost(neighbor, end)
                open_set.put(neighbour, (f_cost, _grid_cost(neighbour, grid)))

            # if we've already got a lower g_score for neighbor, then move on
            elif curr_g_score >= g_cost[neighbor]:
                continue

            prev[neighbor] = curr
            g_cost[neighbor] = curr_g_score

        closed_set.add(curr)

    # if we get to this point, it's not possible to reach the end destination
    return []
Ejemplo n.º 15
0
class Waiting:
    __doc__ = '''
    Class that handles and organizes cars that are not actively moving in the simulation
    '''

    def __init__(self, simulation):
        '''
        Constructor for Waiting

        :param simulation: Simulation - reference to simulation object
        '''
        self.homes = PriorityQueue(key=lambda x: x[0])
        self.simulation = simulation
        self.fixed = False
        self.pathfinder = PathFinder(simulation.ground, cost_function)

    def attach(self, ts, desX, desY, home, hub):
        '''
        Appends a predefined-structured tuple that represents data from the order text file
        which indicates the order and time to move each car.

        Constructs a Car agent object.

        Will raise a non-fatal alert if attempted to attach after being fixed

        :param ts: Int - timestep to begin moving a car
        :param desX: Int - destination x-coordinate
        :param desY: Int - destination y-coordinate
        :param home: Home - reference to the starting Home of the car
        :param hub: Hub - reference to the destination Hub of the car
        :return: None
        '''
        if not self.fixed:
            if not isinstance(home, Home):
                raise ValueError(f'Expected Home tile but given {type(home)}')
            elif not isinstance(hub, Hub):
                raise ValueError(f'Expected Hub tile but given {type(hub)}')

            car = Car(home.x, home.y, desX, desY, hub, home)
            self.homes.push((ts, desX, desY, home, hub, car))
        else:
            warnings.warn('Attempted to attach object to Waiting after fixing')

    def detach(self):
        '''
        Removes the object with the most recent initialization timestep in homes.

        :return: Home/None - Home object of tuple that is removed (None if not fixed)
        '''
        if self.fixed and len(self.homes):
            return self.homes.pop()[HOME]
        return None

    def notify_observer(self, timestep):
        '''
        Attaches all car agents to the timestep object once the current timestep is reached for
        the car agents to start moving.

        :param timestep: Timestep - reference to Timestep object
        :return: None
        '''
        # attaches the car agent to Timestep object whenever it is time for the car to start moving
        #   as specified by order.txt
        while len(self.homes) and self.homes.top(
        )[TIMESTEP] == self.simulation.steps:
            timestep.attach(self.homes.pop()[CAR])

    def fix_state(self, verbose=False):
        '''
        Sorts all tuples in order and loads cars into Home objects.

        This should be called before the simulations starts.

        :param verbose: Boolean - if True, print out message when there are no paths for some cars
        :return: None
        '''
        self.fixed = True

        for home in self.homes:
            ts, desX, desY, h, hub, car = home
            if not h.load_car(car, self.pathfinder) and verbose:
                print(f'Car at {h.get_coord()} does not have a valid path')

    def reset(self):
        '''
        Empties the homes container

        :return: None
        '''
        self.homes.clear()
        self.fixed = False

    def get_size(self):
        '''
        Gets the number of cars in waiting for convenience.

        :return: Int - number of cars waiting
        '''
        return len(self.homes)

    def __str__(self):
        homes_list = list()
        for home in sorted(self.homes.container, key=lambda x: x[0]):
            home_string = f'[{home[TIMESTEP]}] Car ({home[CAR].id}) starts at: {home[HOME].get_coord()} -> {home[HUB].get_coord()}'
            homes_list.append(home_string)

        return '\n'.join(homes_list)
Ejemplo n.º 16
0
    def hybrid_search(self,
                      total_depth_limit=-1,
                      explore_depth_limit=15,
                      n_sols=1,
                      explore_n_sols=-1,
                      option="ddp"):
        """ performs hybrid search

        Depth limited discrete search will be applied to populate the tree and 
        find discrete solutions. Best first search will then run DDP using goal_count
        as heuristics

        Args:
            total_depth_limit : int depth limit, if negative, no limit
            explore_depth_limit: int depth limit for dls
            n_sols: int desired number of solution, if negative, find all
            explore_n_sols: desired number of sols for dls explore
        
        Returns:
            nodes_visited: (integer) number of nodes visited
        """
        frontier = PriorityQueue()
        frontier.push(self.root)
        nodes_visited = 0
        goals = []
        finished = False

        while (len(frontier)):
            cur = frontier.pop()
            if option == "refine":
                cur.option = "ddp"
            else:
                cur.option = option
            nodes_visited += 1
            print("Visiting Node", nodes_visited)
            print("Frontier size:", len(frontier))
            print("Depth", cur.depth)
            print("SubTree Goal Count:", cur.goal_count)

            if cur.parent is not None:
                print(cur.action.name)
                cur.calc_traj()
                print("Action cost:", cur.action_cost)

            if cur.action_cost < 1e16:
                if cur.goal_reached():
                    goals.append(cur)
                    print("Found Goal!", len(goals))

                    if option == "refine":
                        refine_start = time.time()
                        finished = self.refine_goal_traj(cur)

                        self.refinement_time += time.time() - refine_start

                        if not finished:
                            goals.pop()
                    else:
                        finished = (len(goals) >= n_sols >= 0)

                    # if desired number of goals reached, end loop
                    if finished:
                        break
                else:
                    if not (cur.depth >= total_depth_limit >= 0):
                        if total_depth_limit < 0 or total_depth_limit > explore_depth_limit + cur.depth:
                            dls_depth_limit = explore_depth_limit + cur.depth
                        else:
                            dls_depth_limit = total_depth_limit

                        self.dls(cur, dls_depth_limit, explore_n_sols)

                        if dls_depth_limit == total_depth_limit and cur.goal_count == 0:
                            continue

                        frontier.heapify()
                        print("New Nodes Added:", len(cur.children))
                        for ch in cur.children:
                            frontier.push(ch)
                            print(ch.action.name)

            print("\n\n\n")

        return (goals, nodes_visited)
Ejemplo n.º 17
0
    def find_path(self, x, y, desX, desY, car):
        '''
        Runs the A* search algorithm using the heuristic function.

        :param x: Int - current x-coordinate
        :param y: Int - current y-coordinate
        :param desX: Int - destination x-coordinate
        :param desY: Int - destination y-coordinate
        :param car: Car - reference to Car object
        :return: deque/None - return a deque object containing directions in order if a
                              route exists, otherwise None
        '''
        heuristic = self.heuristic

        # format:             fScore,          gScore, current tile, previous tuple, direction
        #                       0                 1          2              3            4
        first_node = (heuristic(x, y, desX,
                                desY), 0, car.cur_road, None, DEFAULT)
        open = PriorityQueue()
        open.push(first_node)  # begins with the original node
        closed = set()

        while not open.empty():
            node = open.pop()
            fScore, gScore, tile, prev, dir = node
            closed.add(tile)

            curX, curY = tile.get_coord()
            if curX == desX and curY == desY:  # destination reached!
                return self.backtrack(node)

            neighbours = tile.neighbours
            for i in range(9):
                tile_neighbour = neighbours[i]

                # skip None
                if not tile_neighbour: continue
                # skip tiles already considered
                if tile_neighbour in closed: continue

                childX, childY = tile_neighbour.get_coord()
                # skip if tile is not traversable (and not the destination)
                if not tile_neighbour.traversable() and not (childX == desX and
                                                             childY == desY):
                    continue

                if i > 3: i -= 1  # calibration due to discrepancy

                # builds upon the cost from the original tile
                gScore_child = gScore + self.get_cost(i)
                # gets relative direction from the original tile
                child_relX, child_relY = KEYMAPPING[i]
                fScore_child = gScore_child + heuristic(
                    child_relX + curX, child_relY + curY, desX, desY)

                # finds results if exists
                find_result = open.find(tile_neighbour,
                                        key=lambda t: t[CUR_TILE])
                if find_result and gScore_child >= find_result[GSCORE]:
                    # skips if results exists and has a higher g-score
                    continue

                # push into priority queue
                open.push(
                    (fScore_child, gScore_child, tile_neighbour, node, i))

        return None  # no route can be found