python3 实现狄克斯特拉(Dijkstra)算法 实现加权最短路径规划

狄克斯特拉算法大致思路是，把每个坐标节点都命一个名(本代码中是1-20)，以及列出每个节点的相邻节点，和到相邻节点的长度(权值)，通过计算得出最短的路径。
实现代码：具体思路详见代码注释

from collections import defaultdict
from heapq import *


def dijkstra_raw(edges, from_node, to_node):
    g = defaultdict(list)
    for l, r, c in edges:
        g[l].append((c, r))
    q, seen = [(0, from_node, ())], set()
    while q:
        (cost, v1, path) = heappop(q)
        if v1 not in seen:
            seen.add(v1)
            path = (v1, path)
            if v1 == to_node:
                return cost, path
            for c, v2 in g.get(v1, ()):
                if v2 not in seen:
                    heappush(q, (cost + c, v2, path))
    return float("inf"), []


def dijkstra(edges, from_node, to_node):
    len_shortest_path = -1
    ret_path = []
    length, path_queue = dijkstra_raw(edges, from_node, to_node)
    if len(path_queue) > 0:
        len_shortest_path = length  ## 1. 先求长度
        ## 2. 分解path_queue，以获得最短路径中的传递节点
        left = path_queue[0]
        ret_path.append(left)  ## 2.1 首先记录目标节点;
        right = path_queue[1]
        while len(right) > 0:
            left = right[0]
            ret_path.append(left)  ## 2.2 记录其他节点，直到源节点。
            right = right[1]
        ret_path