a_star.py

"""
A*アルゴリズムの実装です．
手順はWikipedia(https://ja.wikipedia.org/wiki/A*)の解説と同じです
"""

def generate_path(parents, start_node, goal_node):
    """経路上のノードの親子関係を記録した辞書(parents)から経路を復元する"""
    c = goal_node
    path = [goal_node]
    while c != start_node:
        c = parents[c]
        path.append(c)
    return list(reversed(path))


def a_star(G, start_node, goal_nodes, heuristics=None):
    """
    A*アルゴリズムで最短経路問題を解く．
    ヒューリスティック関数が与えられないときダイクストラ法と同じ動作をする．

    Parameters
    ----------
    G: networkx.Graph
        探索の対象となるグラフ
    start_node: int
        スタートノード
    goal_nodes: set of ints
        ゴールノードの集合
    heuristics: dict
        ノードに対するヒューリスティック関数の値
        ノードnのヒューリスティック関数の値はheuristics[n]で与えられる
    """
    def weight(n, m):
        return G[n][m]['weight']

    h = heuristics

    if heuristics is None:
        h = dict((node, 0) for node in G.nodes())

    # 経路を保存するための辞書
    # あるノードnの親がparents[n]に格納される
    parents = {}

    active_nodes = [start_node]  # 計算中のノードを格納するための配列
    inactive_nodes = []  # 計算済みのノードを格納するための配列

    # ノードnまでの最小コストがf[n]に格納される
    f = {start_node: h[start_node]}

    while len(active_nodes) != 0:
        # 計算中ノードのうちf[n]が最小となるノードnを取り出す
        n = min(active_nodes, key=lambda n: f[n])
        if n in goal_nodes:
            # nがゴールノードに含まれていれば探索を終了する
            return generate_path(parents, start_node, n)
        else:
            inactive_nodes.append(n)
            active_nodes.remove(n)

        for m in G.neighbors(n):
            g = f[n] - h[n]
            f_prime = g + weight(n, m) + h[m]

            if m not in active_nodes and m not in inactive_nodes:
                f[m] = f_prime
                active_nodes.append(m)
                parents[m] = n

            if m in active_nodes and f_prime < f[m]:
                f[m] = f_prime
                parents[m] = n

            if m in inactive_nodes and f_prime < f[m]:
                f[m] = f_prime
                inactive_nodes.remove(m)
                active_nodes.append(m)

    # 計算中のノードのリストが空であれば探索は失敗とする
    raise Exception("Could not reach the goal.")

test_a_star.py

import networkx as nx
from matplotlib import pyplot as plt
from a_star import a_star


def draw_graph(G, values, positions):
    keys = range(0, len(values))
    labels = dict(zip(keys, values))
    nx.draw_networkx_nodes(G, positions, node_size=1200)
    nx.draw_networkx_edges(G, positions)
    nx.draw_networkx_labels(G, positions, labels, font_size=21)
    nx.draw_networkx_edge_labels(G, positions)
    plt.show()


def test_dijkstra():
    S = 0
    T = 6

    G = nx.Graph()

    G.add_nodes_from([S, 1, 2, 3, 4, 5, T])

    G.add_edge(S, 1, weight=2)
    G.add_edge(S, 3, weight=5)
    G.add_edge(S, 4, weight=7)
    G.add_edge(1, 2, weight=5)
    G.add_edge(1, 3, weight=1)
    G.add_edge(2, 3, weight=4)
    G.add_edge(2, T, weight=5)
    G.add_edge(3, 4, weight=3)
    G.add_edge(3, 5, weight=7)
    G.add_edge(3, T, weight=9)
    G.add_edge(4, 5, weight=3)
    G.add_edge(5, T, weight=2)

    path = a_star(G, S, [T])
    print("Dijkstra: " + str(path))
    assert(path == [S, 1, 3, 4, 5, T])

    positions = [[0, 0], [1, 1], [3, 1], [2, 0], [1, -1], [3, -1], [4, 0]]
    values = ['S', 1, 2, 3, 4, 5, 'T']
    draw_graph(G, values, positions)


def test_a_star():
    S = 0
    T = 5

    G = nx.Graph()

    G.add_nodes_from([S, 1, 2, 3, 4, T])

    G.add_edge(S, 1, weight=5)
    G.add_edge(S, 2, weight=3)
    G.add_edge(1, 2, weight=3)
    G.add_edge(1, 3, weight=5)
    G.add_edge(1, 4, weight=1)
    G.add_edge(2, 3, weight=1)
    G.add_edge(2, 4, weight=3)
    G.add_edge(3, 4, weight=2)
    G.add_edge(3, T, weight=1)
    G.add_edge(4, T, weight=4)

    heuristics = {S: 2, 1: 5, 2: 1, 3: 1, 4: 4, 5: 0}
    path = a_star(G, S, [T], heuristics)
    print("A*      : " + str(path))
    assert(path == [S, 2, 3, T])

    positions = [[0, 0], [1, 1], [1, -1], [2, 1], [2, -1], [3, 0]]
    values = ['S', 1, 2, 3, 4, 'T']
    draw_graph(G, values, positions)

test_dijkstra()
test_a_star()