Python PriorityQueue.insert方法代码示例

# 需要导入模块: from queue import PriorityQueue [as 别名]
# 或者: from queue.PriorityQueue import insert [as 别名]
def dstar_lite(problem):
    """Performs D* Lite to find incrementally find a shortest path as the robot
    moves through the graph.  Updates the IncrementalSearchProblem, problem, by
    calling problem.update_world.  The search terminates when the robot either
    reaches the goal, or finds that there is no path to the goal.  Returns the
    modified problem.

    Note: The world is dynamic, so the true positions of obstacles may change as
    the robot moves through the world.  However, if the robot determines at any
    point that there is no finite path to the goal, it should stop searching and
    give up, rather than waiting and hoping that the world will improve.
    """

    ############################################################################
    # INITIALIZE

    # Get the start node, goal node, and graph from the IncrementalSearchProblem
    start = problem.start_node
    goal = problem.goal_node
    graph = problem.get_graph()

    # Set g=inf and rhs=inf for all nodes, except the goal node, which has rhs=0
    g = {node:inf for node in graph.get_all_nodes()}
    rhs = {node:inf for node in graph.get_all_nodes()}
    rhs[goal] = 0

    # Set the key modifier k_m to 0
    key_modifier = 0

    # Define shortened helper functions
    def calc_key(node):
        return calc_key_helper(node, g, rhs, start, key_modifier)
    queue = None # to be reinitialized later
    def update_vertex(node):
        return update_vertex_helper(node, g, rhs, goal, graph, queue)
    def compute_shortest_path():
        return compute_shortest_path_helper(g, rhs, start, goal, key_modifier, graph, queue)

    # Initialize the queue using the priority function calc_key
    queue = PriorityQueue(f=lambda node: calc_key(node))
    queue.insert(goal)

    ############################################################################
    # YOUR CODE HERE
    raise NotImplementedError

    return problem

# 需要导入模块: from queue import PriorityQueue [as 别名]
# 或者: from queue.PriorityQueue import insert [as 别名]
def test_compute_shortest_path_helper(compute_shortest_path_helper,calc_key_helper):
    print "Testing compute_shortest_path_helper..."

    # problem_simple, first step
    g = {(1, 2): inf, (0, 1): inf, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): inf, (0, 2): inf, (2, 0): inf, (1, 3): inf, (2, 3): inf, (2, 1): inf, (2, 2): inf, (1, 0): inf, (4, 2): inf, (0, 3): inf, (0, 4): inf, (4, 1): inf, (1, 1): inf, (4, 0): inf}
    rhs = {(1, 2): inf, (0, 1): inf, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): inf, (0, 2): inf, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): inf, (2, 2): inf, (1, 0): inf, (4, 2): inf, (0, 3): inf, (0, 4): inf, (4, 1): inf, (1, 1): inf, (4, 0): inf}
    graph = graph_simple.copy()
    calc_key = lambda node: calc_key_helper(node, g, rhs, (0, 3), 0)
    queue = PriorityQueue(f=lambda node: calc_key(node))
    queue.insert(goal_simple)
    g_expected = {(1, 2): inf, (0, 1): inf, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): inf, (0, 2): 2.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): inf, (1, 0): inf, (4, 2): inf, (0, 3): 3.0, (0, 4): inf, (4, 1): inf, (1, 1): 1.0, (4, 0): inf}
    rhs_expected = {(1, 2): inf, (0, 1): 2.0, (3, 2): inf, (0, 0): 2.0, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): 4.0, (0, 2): 2.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): 2.0, (1, 0): 1.0, (4, 2): inf, (0, 3): 3.0, (0, 4): 4.0, (4, 1): inf, (1, 1): 1.0, (4, 0): inf}
    compute_shortest_path_helper(g, rhs, (0, 3), goal_simple, 0, graph, queue)
    assert graph == graph_simple #not modified
    assert g == g_expected
    assert rhs == rhs_expected
    queue_expected = PriorityQueue(f=lambda node: calc_key(node))
    queue_expected.extend([(1, 0), (0, 1), (2, 2), (0, 0), (0, 4), (1, 4)])
    assert queue == queue_expected

    # problem_simple, second step
    g = {(1, 2): inf, (0, 1): inf, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): inf, (0, 2): 2.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): inf, (1, 0): inf, (4, 2): inf, (0, 3): 3.0, (0, 4): inf, (4, 1): inf, (1, 1): 1.0, (4, 0): inf}
    rhs = {(1, 2): inf, (0, 1): 3.0, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): 4.0, (0, 2): 4.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): 2.0, (1, 0): 1.0, (4, 2): inf, (0, 3): 3.0, (0, 4): 4.0, (4, 1): inf, (1, 1): inf, (4, 0): inf}
    graph = graph_simple_step2.copy()
    calc_key = lambda node: calc_key_helper(node, g, rhs, (0, 2), 1)
    queue = PriorityQueue(f=lambda node: calc_key(node))
    queue.extend([(1, 1), (0, 2), (1, 0), (2, 2), (0, 1), (0, 4), (1, 4)])
    g_expected = {(1, 2): inf, (0, 1): 2.0, (3, 2): inf, (0, 0): inf, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): inf, (0, 2): 3.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): inf, (1, 0): 1.0, (4, 2): inf, (0, 3): 3.0, (0, 4): inf, (4, 1): inf, (1, 1): inf, (4, 0): inf}
    rhs_expected = {(1, 2): inf, (0, 1): 2.0, (3, 2): inf, (0, 0): 2.0, (3, 0): inf, (3, 1): inf, (2, 4): inf, (1, 4): 4.0, (0, 2): 3.0, (2, 0): 0, (1, 3): inf, (2, 3): inf, (2, 1): 1.0, (2, 2): 2.0, (1, 0): 1.0, (4, 2): inf, (0, 3): 4.0, (0, 4): 4.0, (4, 1): inf, (1, 1): inf, (4, 0): inf}
    compute_shortest_path_helper(g, rhs, (0, 2), goal_simple, 1, graph, queue)
    assert graph == graph_simple_step2 #not modified
    assert g == g_expected
    assert rhs == rhs_expected
    queue_expected = PriorityQueue(f=lambda node: calc_key(node))
    queue_expected.extend([(0, 0), (2, 2), (0, 3), (0, 4), (1, 4)])
    assert queue == queue_expected

# 需要导入模块: from queue import PriorityQueue [as 别名]
# 或者: from queue.PriorityQueue import insert [as 别名]
def dstar_lite(problem):

    # Initialize
    start = problem.start_node
    goal = problem.goal_node
    graph = problem.get_graph()

    g = {node:inf for node in graph.get_all_nodes()}
    rhs = {node:inf for node in graph.get_all_nodes()}
    rhs[goal] = 0
    key_modifier = 0

    def calc_key(node):
        "Returns key as a tuple of two ints"
        min_g_rhs = min([g[node], rhs[node]])
        return (min_g_rhs + grid_heuristic(start, node) + key_modifier, min_g_rhs)

    queue = PriorityQueue(f=lambda node: calc_key(node))
    queue.insert(goal)

    def update_vertex(node):
        if node != goal:
            rhs[node] = min([graph.get_edge_weight(node, neighbor) + g[neighbor]
                             for neighbor in graph.get_successors(node)])
        if node in queue:
            queue.remove(node)
        if g[node] != rhs[node]:
            queue.insert(node)

    def compute_shortest_path():
        print '> COMPUTE SHORTEST PATH'
        while True:
            smallest_key = queue.top_key()
            if smallest_key >= calc_key(start) and rhs[start] == g[start]:
                return
            node = queue.pop()
            print '> dequeue node', node, 'with h =', grid_heuristic(node, start)
            if smallest_key < calc_key(node):
                queue.insert(node)
            elif g[node] > rhs[node]:
                g[node] = rhs[node]
                for next_node in graph.get_predecessors(node):
                    update_vertex(next_node)
            else:
                g[node] = inf
                for next_node in graph.get_predecessors(node) + [node]:
                    update_vertex(next_node)


    # Begin algorithm
    last_start = start
    compute_shortest_path()
    print 'robot starting at:', start

    while start != goal:
        if g[start] == inf:
            print "no path found"
            return problem
        start = min(graph.get_successors(start),
                    key = lambda neighbor: (graph.get_edge_weight(start, neighbor)
                                            + g[neighbor]))
        old_graph = graph.copy()
        print 'robot moving to:', start
        intended_path = build_intended_path(start, goal, graph, g)
        print 'intended path:', intended_path
        graph = problem.update_world(intended_path)
        changed_edges = old_graph.get_changed_edges(graph)
        if changed_edges:
            key_modifier = key_modifier + grid_heuristic(last_start, start)
            last_start = start
            for (old_edge, new_edge) in changed_edges:
                if old_edge and new_edge: #edge simply changed weight
                    update_vertex(old_edge.source)
                elif not old_edge: #new edge was added
                    raise NotImplementedError("Edge addition not yet supported")
                else: #old edge was deleted
                    raise NotImplementedError("Edge deletion not yet supported")
            compute_shortest_path()
    print 'robot at:', start
    return problem #contains path traversed, intended future path, and other info

# 需要导入模块: from queue import PriorityQueue [as 别名]
# 或者: from queue.PriorityQueue import insert [as 别名]
"""
ASTR 598 HW 5
Brett Morris
"""
from __future__ import print_function  # Python 2 compatibility

from queue import PriorityQueue

q = PriorityQueue()

# Twenty randomly shuffled integers from 1 to 21
integers = [16, 13, 10, 12, 15, 6, 11, 3, 19, 1, 20, 9, 7, 2, 5, 4, 14, 18, 17, 8]

# Insert random integers into binary min heap
for integer in integers:
    q.insert(integer)

# Remove minimum from heap until it's empty
for i in range(len(integers)):
    print(q.del_min())

# 需要导入模块: from queue import PriorityQueue [as 别名]
# 或者: from queue.PriorityQueue import insert [as 别名]
def dstar_lite(problem):
    """Performs D* Lite to find incrementally find a shortest path as the robot
    moves through the graph.  Updates the IncrementalSearchProblem, problem, by
    calling problem.update_world.  The search terminates when the robot either
    reaches the goal, or finds that there is no path to the goal.  Returns the
    modified problem.

    Note: The world is dynamic, so the true positions of obstacles may change as
    the robot moves through the world.  However, if the robot determines at any
    point that there is no finite path to the goal, it should stop searching and
    give up, rather than waiting and hoping that the world will improve.
    """

    ############################################################################
    # INITIALIZE

    # Get the start node, goal node, and graph from the IncrementalSearchProblem
    start = problem.start_node
    goal = problem.goal_node
    graph = problem.get_graph()

    # Set g=inf and rhs=inf for all nodes, except the goal node, which has rhs=0
    g = {node:inf for node in graph.get_all_nodes()}
    rhs = {node:inf for node in graph.get_all_nodes()}
    rhs[goal] = 0

    # Set the key modifier k_m to 0
    key_modifier = 0

    # Define shortened helper functions
    def calc_key(node):
        return calc_key_helper(node, g, rhs, start, key_modifier)
    queue = None # to be reinitialized later
    def update_vertex(node):
        return update_vertex_helper(node, g, rhs, goal, graph, queue)
    def compute_shortest_path():
        return compute_shortest_path_helper(g, rhs, start, goal, key_modifier, graph, queue)

    # Initialize the queue using the priority function calc_key
    queue = PriorityQueue(f=lambda node: calc_key(node))
    queue.insert(goal)

    ############################################################################
    # YOUR CODE HERE

    # Begin algorithm
    last_start = start
    compute_shortest_path()
    print 'robot starting at:', start

    while start != goal:
        if g[start] == inf:
            print "no path found"
            return problem
        start = min(graph.get_successors(start),
                    key = lambda neighbor: (graph.get_edge_weight(start, neighbor)
                                            + g[neighbor]))
        old_graph = graph.copy()
        print 'robot moving to:', start
        intended_path = get_intended_path(start, goal, graph, g)
        print 'intended path:', intended_path
        graph = problem.update_world(intended_path)
        changed_edges = old_graph.get_changed_edges(graph)
        if changed_edges:
            key_modifier = key_modifier + grid_heuristic(last_start, start)
            last_start = start
            for (old_edge, new_edge) in changed_edges:
                if old_edge and new_edge: #edge simply changed weight
                    update_vertex(old_edge.source)
                elif not old_edge: #new edge was added
                    raise NotImplementedError("Edge addition not yet supported")
                else: #old edge was deleted
                    raise NotImplementedError("Edge deletion not yet supported")
            compute_shortest_path()
    print 'robot at:', start
    return problem #contains path traversed, intended future path, and other info