Python networkx.astar_path() Examples

def test_random_graph(self):

        def dist(a, b):
            (x1, y1) = a
            (x2, y2) = b
            return ((x1 - x2) ** 2 + (y1 - y2) ** 2) ** 0.5

        G = nx.Graph()

        points = [(random(), random()) for _ in range(100)]

        # Build a path from points[0] to points[-1] to be sure it exists
        for p1, p2 in zip(points[:-1], points[1:]):
            G.add_edge(p1, p2, weight=dist(p1, p2))

        # Add other random edges
        for _ in range(100):
            p1, p2 = choice(points), choice(points)
            G.add_edge(p1, p2, weight=dist(p1, p2))

        path = nx.astar_path(G, points[0], points[-1], dist)
        assert path == nx.dijkstra_path(G, points[0], points[-1]) 

def path_search(self, origin, destination):
        """
        This function finds the shortest path connecting origin and destination
        using A* search with distance heuristic.
        origin      :   tuple containing x, y co-ordinates of start position
        desitnation :   tuple containing x, y co-ordinates of end position
        return      :   path as list of node ids (as int) of the graph self._graph
        connecting origin and destination
        """
        xo, yo = origin
        xd, yd = destination
        start = self.localise(xo, yo)
        end = self.localise(xd, yd)

        route = nx.astar_path(
            self._graph, source=self._id_map[start['entry']],
            target=self._id_map[end['exit']],
            heuristic=self._distance_heuristic,
            weight='length')

        return route 

def test_random_graph(self):
        """Tests that the A* shortest path agrees with Dijkstra's
        shortest path for a random graph.

        """

        G = nx.Graph()

        points = [(random(), random()) for _ in range(100)]

        # Build a path from points[0] to points[-1] to be sure it exists
        for p1, p2 in pairwise(points):
            G.add_edge(p1, p2, weight=dist(p1, p2))

        # Add other random edges
        for _ in range(100):
            p1, p2 = choice(points), choice(points)
            G.add_edge(p1, p2, weight=dist(p1, p2))

        path = nx.astar_path(G, points[0], points[-1], dist)
        assert_equal(path, nx.dijkstra_path(G, points[0], points[-1])) 

def test_astar_undirected3(self):
        XG4=nx.Graph()
        XG4.add_edges_from([ [0,1,{'weight':2}],
                             [1,2,{'weight':2}],
                             [2,3,{'weight':1}],
                             [3,4,{'weight':1}],
                             [4,5,{'weight':1}],
                             [5,6,{'weight':1}],
                             [6,7,{'weight':1}],
                             [7,0,{'weight':1}] ])
        assert nx.astar_path(XG4,0,2)==[0, 1, 2]
        assert nx.astar_path_length(XG4,0,2)==4


# >>> MXG4=NX.MultiGraph(XG4)
# >>> MXG4.add_edge(0,1,3)
# >>> NX.dijkstra_path(MXG4,0,2)
# [0, 1, 2] 

def test_random_graph(self):
        """Tests that the A* shortest path agrees with Dijkstra's
        shortest path for a random graph.

        """

        G = nx.Graph()

        points = [(random(), random()) for _ in range(100)]

        # Build a path from points[0] to points[-1] to be sure it exists
        for p1, p2 in pairwise(points):
            G.add_edge(p1, p2, weight=dist(p1, p2))

        # Add other random edges
        for _ in range(100):
            p1, p2 = choice(points), choice(points)
            G.add_edge(p1, p2, weight=dist(p1, p2))

        path = nx.astar_path(G, points[0], points[-1], dist)
        assert_equal(path, nx.dijkstra_path(G, points[0], points[-1])) 

def _path_search(self, origin, destination):
        """
        This function finds the shortest path connecting origin and destination
        using A* search with distance heuristic.
        origin      :   carla.Location object of start position
        destination :   carla.Location object of of end position
        return      :   path as list of node ids (as int) of the graph self._graph
        connecting origin and destination
        """

        start, end = self._localize(origin), self._localize(destination)

        route = nx.astar_path(
            self._graph, source=start[0], target=end[0],
            heuristic=self._distance_heuristic, weight='length')
        route.append(end[1])
        return route 

def test_astar_undirected3(self):
        XG4 = nx.Graph()
        edges = [(0, 1, 2), (1, 2, 2), (2, 3, 1), (3, 4, 1), (4, 5, 1),
                 (5, 6, 1), (6, 7, 1), (7, 0, 1)]
        XG4.add_weighted_edges_from(edges)
        assert_equal(nx.astar_path(XG4, 0, 2), [0, 1, 2])
        assert_equal(nx.astar_path_length(XG4, 0, 2), 4)

# >>> MXG4=NX.MultiGraph(XG4)
# >>> MXG4.add_edge(0,1,3)
# >>> NX.dijkstra_path(MXG4,0,2)
# [0, 1, 2] 

def test_cycle(self):
        C = nx.cycle_graph(7)
        assert_equal(nx.astar_path(C, 0, 3), [0, 1, 2, 3])
        assert_equal(nx.dijkstra_path(C, 0, 4), [0, 6, 5, 4]) 

def test_astar_w1(self):
        G = nx.DiGraph()
        G.add_edges_from([('s', 'u'), ('s', 'x'), ('u', 'v'), ('u', 'x'),
                          ('v', 'y'), ('x', 'u'), ('x', 'w'), ('w', 'v'),
                          ('x', 'y'), ('y', 's'), ('y', 'v')])
        assert_equal(nx.astar_path(G, 's', 'v'), ['s', 'u', 'v'])
        assert_equal(nx.astar_path_length(G, 's', 'v'), 2) 

def test_astar_nopath(self):
        nx.astar_path(self.XG, 's', 'moon') 

def test_cycle(self):
        C = nx.cycle_graph(7)
        assert_equal(nx.astar_path(C, 0, 3), [0, 1, 2, 3])
        assert_equal(nx.dijkstra_path(C, 0, 4), [0, 6, 5, 4]) 

def astar_path_length(G, source, target, heuristic=None, weight='weight'):
    """Return the length of the shortest path between source and target using
    the A* ("A-star") algorithm.

    Parameters
    ----------
    G : NetworkX graph

    source : node
       Starting node for path

    target : node
       Ending node for path

    heuristic : function
       A function to evaluate the estimate of the distance
       from the a node to the target.  The function takes
       two nodes arguments and must return a number.

    Raises
    ------
    NetworkXNoPath
        If no path exists between source and target.

    See Also
    --------
    astar_path

    """
    if source not in G or target not in G:
        msg = 'Either source {} or target {} is not in G'
        raise nx.NodeNotFound(msg.format(source, target))

    path = astar_path(G, source, target, heuristic, weight)
    return sum(G[u][v].get(weight, 1) for u, v in zip(path[:-1], path[1:])) 

def test_astar_directed(self):
        assert_equal(nx.astar_path(self.XG, 's', 'v'), ['s', 'x', 'u', 'v'])
        assert_equal(nx.astar_path_length(self.XG, 's', 'v'), 9) 

def test_astar_multigraph(self):
        G = nx.MultiDiGraph(self.XG)
        assert_raises(nx.NetworkXNotImplemented, nx.astar_path, G, 's', 'v')
        assert_raises(nx.NetworkXNotImplemented, nx.astar_path_length,
                      G, 's', 'v') 

def test_astar_undirected(self):
        GG = self.XG.to_undirected()
        # make sure we get lower weight
        # to_undirected might choose either edge with weight 2 or weight 3
        GG['u']['x']['weight'] = 2
        GG['y']['v']['weight'] = 2
        assert_equal(nx.astar_path(GG, 's', 'v'), ['s', 'x', 'u', 'v'])
        assert_equal(nx.astar_path_length(GG, 's', 'v'), 8) 

def test_astar_undirected2(self):
        XG3 = nx.Graph()
        edges = [(0, 1, 2), (1, 2, 12), (2, 3, 1), (3, 4, 5), (4, 5, 1),
                 (5, 0, 10)]
        XG3.add_weighted_edges_from(edges)
        assert_equal(nx.astar_path(XG3, 0, 3), [0, 1, 2, 3])
        assert_equal(nx.astar_path_length(XG3, 0, 3), 15) 

def test_astar_undirected3(self):
        XG4 = nx.Graph()
        edges = [(0, 1, 2), (1, 2, 2), (2, 3, 1), (3, 4, 1), (4, 5, 1),
                 (5, 6, 1), (6, 7, 1), (7, 0, 1)]
        XG4.add_weighted_edges_from(edges)
        assert_equal(nx.astar_path(XG4, 0, 2), [0, 1, 2])
        assert_equal(nx.astar_path_length(XG4, 0, 2), 4)

# >>> MXG4=NX.MultiGraph(XG4)
# >>> MXG4.add_edge(0,1,3)
# >>> NX.dijkstra_path(MXG4,0,2)
# [0, 1, 2] 

def test_astar_w1(self):
        G = nx.DiGraph()
        G.add_edges_from([('s', 'u'), ('s', 'x'), ('u', 'v'), ('u', 'x'),
                          ('v', 'y'), ('x', 'u'), ('x', 'w'), ('w', 'v'),
                          ('x', 'y'), ('y', 's'), ('y', 'v')])
        assert_equal(nx.astar_path(G, 's', 'v'), ['s', 'u', 'v'])
        assert_equal(nx.astar_path_length(G, 's', 'v'), 2) 

def test_astar_nopath(self):
        nx.astar_path(self.XG, 's', 'moon') 

def path(self, src: dict, dst: dict):
        if not self.__is_dst(src, dst):
            return nx.astar_path(self.directed, src, dst) 

def test_astar_undirected2(self):
        XG3 = nx.Graph()
        edges = [(0, 1, 2), (1, 2, 12), (2, 3, 1), (3, 4, 5), (4, 5, 1),
                 (5, 0, 10)]
        XG3.add_weighted_edges_from(edges)
        assert_equal(nx.astar_path(XG3, 0, 3), [0, 1, 2, 3])
        assert_equal(nx.astar_path_length(XG3, 0, 3), 15) 

def test_astar_undirected(self):
        GG = self.XG.to_undirected()
        # make sure we get lower weight
        # to_undirected might choose either edge with weight 2 or weight 3
        GG['u']['x']['weight'] = 2
        GG['y']['v']['weight'] = 2
        assert_equal(nx.astar_path(GG, 's', 'v'), ['s', 'x', 'u', 'v'])
        assert_equal(nx.astar_path_length(GG, 's', 'v'), 8) 

def test_astar_multigraph(self):
        G = nx.MultiDiGraph(self.XG)
        assert_raises(nx.NetworkXNotImplemented, nx.astar_path, G, 's', 'v')
        assert_raises(nx.NetworkXNotImplemented, nx.astar_path_length,
                      G, 's', 'v') 

def test_astar_directed(self):
        assert_equal(nx.astar_path(self.XG, 's', 'v'), ['s', 'x', 'u', 'v'])
        assert_equal(nx.astar_path_length(self.XG, 's', 'v'), 9) 

def astar_path_length(G, source, target, heuristic=None, weight='weight'):
    """Returns the length of the shortest path between source and target using
    the A* ("A-star") algorithm.

    Parameters
    ----------
    G : NetworkX graph

    source : node
       Starting node for path

    target : node
       Ending node for path

    heuristic : function
       A function to evaluate the estimate of the distance
       from the a node to the target.  The function takes
       two nodes arguments and must return a number.

    Raises
    ------
    NetworkXNoPath
        If no path exists between source and target.

    See Also
    --------
    astar_path

    """
    if source not in G or target not in G:
        msg = 'Either source {} or target {} is not in G'
        raise nx.NodeNotFound(msg.format(source, target))

    path = astar_path(G, source, target, heuristic, weight)
    return sum(G[u][v].get(weight, 1) for u, v in zip(path[:-1], path[1:])) 

def get_shortest_path(self, pose, goal_pose):
        path = nx.astar_path(self.graph, pose[:3], goal_pose[:3],
                heuristic=lambda nodea, nodeb: (abs(nodea[0] - nodeb[0]) + abs(nodea[1] - nodeb[1])),
                weight='weight')

        # Remove last rotations
        '''
        if not constants.USE_NAVIGATION_AGENT:
            while len(path) > 1:
                if (path[-1][0] == path[-2][0] and
                    path[-1][1] == path[-2][1]):
                    path.pop()
                else:
                    break
        '''

        max_point = 1
        for ii in range(len(path) - 1):
            weight = self.graph[path[ii]][path[ii + 1]]['weight']
            if path[ii][:2] != path[ii + 1][:2]:
                if abs(self.memory[path[ii + 1][1] - self.yMin, path[ii + 1][0] - self.xMin, 0] - weight) > 0.001:
                    print(self.memory[path[ii + 1][1] - self.yMin, path[ii + 1][0] - self.xMin, 0], weight)
                    if constants.USE_NAVIGATION_AGENT:
                        print('nxgraph weights and memory do not match, check that both were updated at all times.')
                    else:
                        print('constants.USE_NAVIGATION_AGENT was False. It may need to be true to get the shortest path.')
                    pdb.set_trace()
            if weight == MAX_WEIGHT:
                break
            max_point += 1
        path = path[:max_point]

        actions = [self.get_plan_move(path[ii], path[ii + 1]) for ii in range(len(path) - 1)]
        return actions, path 

def test_cycle(self):
        C=nx.cycle_graph(7)
        assert nx.astar_path(C,0,3)==[0, 1, 2, 3]
        assert nx.dijkstra_path(C,0,4)==[0, 6, 5, 4] 

def test_astar_nopath(self):
        p = nx.astar_path(self.XG,'s','moon') 

def test_astar_w1(self):
        G=nx.DiGraph()
        G.add_edges_from([('s','u'), ('s','x'), ('u','v'), ('u','x'),
            ('v','y'), ('x','u'), ('x','w'), ('w', 'v'), ('x','y'),
            ('y','s'), ('y','v')])
        assert nx.astar_path(G,'s','v')==['s', 'u', 'v']
        assert nx.astar_path_length(G,'s','v')== 2 

def test_astar_undirected2(self):
        XG3=nx.Graph()
        XG3.add_edges_from([ [0,1,{'weight':2}],
                             [1,2,{'weight':12}],
                             [2,3,{'weight':1}],
                             [3,4,{'weight':5}],
                             [4,5,{'weight':1}],
                             [5,0,{'weight':10}] ])
        assert nx.astar_path(XG3,0,3)==[0, 1, 2, 3]
        assert nx.astar_path_length(XG3,0,3)==15