[Python][library] A* path search

astar.py

def astar(init, goal, nexts,
          distance=lambda path: len(path),
          heuristic=lambda pos: 0):
    import heapq
    queue = []
    init_score = distance([init]) + heuristic(init)
    checked = {init: init_score}
    heapq.heappush(queue, (init_score, [init]))
    while len(queue) > 0:
        score, path = heapq.heappop(queue)
        last = path[-1]
        if last == goal: return path
        for pos in nexts(last):
            newpath = path + [pos]
            pos_score = distance(newpath) + heuristic(pos)
            if pos in checked and checked[pos] <= pos_score: continue
            checked[pos] = pos_score
            heapq.heappush(queue, (pos_score, newpath))
            pass
        pass
    return None

if __name__ == "__main__":
    # from: http://d.hatena.ne.jp/pashango_p/20090713/1247455609
    dungeon = [
        'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
        'OS  O     O     O         O          O',
        'O   O  O  O  O  O         O    OOOO GO',
        'O      O     O  O   OOOO  O    O  OOOO',
        'OO OOOOOOOOOOOOOOO  O     O    O     O',
        'O                O  O     O          O',
        'O        OOO     O  O     OOOOOOOOO  O',
        'O  OO    O    OOOO  O     O      OO  O',
        'O   O    O          O     O  O   O   O',
        'O   OOO  O          O        O   O   O',
        'O        O          O        O       O',
        'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
        ]

    def find_ch(ch):
        for i, l in enumerate(dungeon):
            for j, c in enumerate(l):
                if c == ch: return (i, j)
                pass
            pass
        return None

    init = find_ch("S")
    goal = find_ch("G")
    def nexts(pos):
        wall = "O"
        if dungeon[pos[0] - 1][pos[1]] != wall: yield (pos[0] - 1, pos[1])
        if dungeon[pos[0] + 1][pos[1]] != wall: yield (pos[0] + 1, pos[1])
        if dungeon[pos[0]][pos[1] - 1] != wall: yield (pos[0], pos[1] - 1)
        if dungeon[pos[0]][pos[1] + 1] != wall: yield (pos[0], pos[1] + 1)
        pass
    def heuristic(pos):
        return ((pos[0] - goal[0]) ** 2 + (pos[1] - goal[1]) ** 2) ** 0.5
    def distance(path):
        return len(path)
    def render_path(path):
        buf = [[ch for ch in l] for l in dungeon]
        for pos in path[1:-1]: buf[pos[0]][pos[1]] = "*"
        buf[path[0][0]][path[0][1]] = "s"
        buf[path[-1][0]][path[-1][1]] = "g"
        return ["".join(l) for l in buf]

    path = astar(init, goal, nexts, distance, heuristic)
    #path = astar(init, goal, nexts)
    print "\n".join(render_path(path))
    pass