Skip to content

Dijkstra

src.python_motion_planning.path_planner.graph_search.dijkstra.Dijkstra

Bases: BasePathPlanner

Class for Dijkstra path planner.

Parameters:

Name Type Description Default
*args

see the parent class.

 ()
*kwargs

see the parent class.

 {}
References

[1] A Note on Two Problems in Connexion with Graphs

Examples:

Python Console Session
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
>>> planner = Dijkstra(map_=map_, start=(5, 5), goal=(10, 10))
>>> planner.plan()
([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 0), (4, 5): Node((4, 5), (5, 5), 1.0, 0), (6, 5): Node((6, 5), (5, 5), 1.0, 0), (5, 4): Node((5, 4), (5, 5), 1.0, 0), (5, 6): Node((5, 6), (5, 5), 1.0, 0), (4, 6): Node((4, 6), (5, 5), 1.4142135623730951, 0), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 0), (6, 4): Node((6, 4), (5, 5), 1.4142135623730951, 0), (4, 4): Node((4, 4), (5, 5), 1.4142135623730951, 0), (5, 7): Node((5, 7), (5, 6), 2.0, 0), (5, 3): Node((5, 3), (5, 4), 2.0, 0), (3, 5): Node((3, 5), (4, 5), 2.0, 0), (7, 5): Node((7, 5), (6, 5), 2.0, 0), (4, 7): Node((4, 7), (4, 6), 2.414213562373095, 0), (3, 6): Node((3, 6), (4, 6), 2.414213562373095, 0), (6, 7): Node((6, 7), (5, 6), 2.414213562373095, 0), (4, 3): Node((4, 3), (4, 4), 2.414213562373095, 0), (6, 3): Node((6, 3), (5, 4), 2.414213562373095, 0), (3, 4): Node((3, 4), (4, 4), 2.414213562373095, 0), (7, 6): Node((7, 6), (6, 5), 2.414213562373095, 0), (7, 4): Node((7, 4), (6, 4), 2.414213562373095, 0), (3, 7): Node((3, 7), (4, 6), 2.8284271247461903, 0), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 0), (3, 3): Node((3, 3), (4, 4), 2.8284271247461903, 0), (7, 3): Node((7, 3), (6, 4), 2.8284271247461903, 0), (8, 5): Node((8, 5), (7, 5), 3.0, 0), (2, 5): Node((2, 5), (3, 5), 3.0, 0), (5, 2): Node((5, 2), (5, 3), 3.0, 0), (5, 8): Node((5, 8), (5, 7), 3.0, 0), (4, 2): Node((4, 2), (4, 3), 3.414213562373095, 0), (6, 8): Node((6, 8), (5, 7), 3.414213562373095, 0), (8, 6): Node((8, 6), (7, 6), 3.414213562373095, 0), (8, 4): Node((8, 4), (7, 5), 3.414213562373095, 0), (4, 8): Node((4, 8), (4, 7), 3.414213562373095, 0), (2, 6): Node((2, 6), (3, 6), 3.414213562373095, 0), (2, 4): Node((2, 4), (3, 5), 3.414213562373095, 0), (6, 2): Node((6, 2), (6, 3), 3.414213562373095, 0), (3, 2): Node((3, 2), (4, 3), 3.82842712474619, 0), (2, 7): Node((2, 7), (3, 6), 3.82842712474619, 0), (3, 8): Node((3, 8), (4, 7), 3.82842712474619, 0), (7, 8): Node((7, 8), (6, 7), 3.82842712474619, 0), (8, 7): Node((8, 7), (7, 6), 3.82842712474619, 0), (7, 2): Node((7, 2), (6, 3), 3.82842712474619, 0), (8, 3): Node((8, 3), (7, 4), 3.82842712474619, 0), (2, 3): Node((2, 3), (3, 4), 3.82842712474619, 0), (9, 5): Node((9, 5), (8, 5), 4.0, 0), (5, 9): Node((5, 9), (5, 8), 4.0, 0), (5, 1): Node((5, 1), (5, 2), 4.0, 0), (1, 5): Node((1, 5), (2, 5), 4.0, 0), (8, 2): Node((8, 2), (7, 3), 4.242640687119286, 0), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 0), (2, 8): Node((2, 8), (3, 7), 4.242640687119286, 0), (2, 2): Node((2, 2), (3, 3), 4.242640687119286, 0), (9, 4): Node((9, 4), (8, 5), 4.414213562373095, 0), (9, 6): Node((9, 6), (8, 5), 4.414213562373095, 0), (4, 1): Node((4, 1), (4, 2), 4.414213562373095, 0), (1, 6): Node((1, 6), (2, 6), 4.414213562373095, 0), (4, 9): Node((4, 9), (5, 8), 4.414213562373095, 0), (6, 9): Node((6, 9), (6, 8), 4.414213562373095, 0), (6, 1): Node((6, 1), (5, 2), 4.414213562373095, 0), (1, 4): Node((1, 4), (2, 5), 4.414213562373095, 0), (1, 7): Node((1, 7), (2, 6), 4.82842712474619, 0), (7, 1): Node((7, 1), (7, 2), 4.82842712474619, 0), (7, 9): Node((7, 9), (6, 8), 4.82842712474619, 0), (9, 3): Node((9, 3), (8, 4), 4.82842712474619, 0), (3, 1): Node((3, 1), (3, 2), 4.82842712474619, 0), (9, 7): Node((9, 7), (8, 6), 4.82842712474619, 0), (1, 3): Node((1, 3), (2, 3), 4.82842712474619, 0), (3, 9): Node((3, 9), (3, 8), 4.82842712474619, 0), (5, 10): Node((5, 10), (5, 9), 5.0, 0), (0, 5): Node((0, 5), (1, 5), 5.0, 0), (10, 5): Node((10, 5), (9, 5), 5.0, 0), (5, 0): Node((5, 0), (5, 1), 5.0, 0), (2, 9): Node((2, 9), (3, 8), 5.242640687119285, 0), (8, 1): Node((8, 1), (7, 2), 5.242640687119285, 0), (1, 8): Node((1, 8), (2, 7), 5.242640687119285, 0), (2, 1): Node((2, 1), (3, 2), 5.242640687119285, 0), (8, 9): Node((8, 9), (7, 8), 5.242640687119285, 0), (9, 8): Node((9, 8), (8, 7), 5.242640687119285, 0), (9, 2): Node((9, 2), (8, 3), 5.242640687119285, 0), (1, 2): Node((1, 2), (2, 3), 5.242640687119285, 0), (0, 6): Node((0, 6), (1, 6), 5.414213562373095, 0), (4, 0): Node((4, 0), (4, 1), 5.414213562373095, 0), (10, 6): Node((10, 6), (9, 6), 5.414213562373095, 0), (10, 4): Node((10, 4), (9, 4), 5.414213562373095, 0), (0, 4): Node((0, 4), (1, 5), 5.414213562373095, 0), (4, 10): Node((4, 10), (5, 9), 5.414213562373095, 0), (6, 10): Node((6, 10), (5, 9), 5.414213562373095, 0), (6, 0): Node((6, 0), (5, 1), 5.414213562373095, 0), (1, 1): Node((1, 1), (2, 2), 5.656854249492381, 0), (9, 1): Node((9, 1), (8, 2), 5.656854249492381, 0), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 0), (1, 9): Node((1, 9), (2, 8), 5.656854249492381, 0), (0, 3): Node((0, 3), (1, 4), 5.82842712474619, 0), (3, 0): Node((3, 0), (4, 1), 5.82842712474619, 0), (10, 3): Node((10, 3), (9, 4), 5.82842712474619, 0), (10, 7): Node((10, 7), (9, 6), 5.82842712474619, 0), (7, 0): Node((7, 0), (7, 1), 5.82842712474619, 0), (0, 7): Node((0, 7), (1, 7), 5.82842712474619, 0), (3, 10): Node((3, 10), (3, 9), 5.82842712474619, 0), (7, 10): Node((7, 10), (6, 9), 5.82842712474619, 0), (5, 11): Node((5, 11), (5, 10), 6.0, 0), (11, 5): Node((11, 5), (10, 5), 6.0, 0), (8, 0): Node((8, 0), (8, 1), 6.242640687119285, 0), (10, 8): Node((10, 8), (9, 8), 6.242640687119285, 0), (0, 2): Node((0, 2), (1, 3), 6.242640687119285, 0), (8, 10): Node((8, 10), (7, 9), 6.242640687119285, 0), (0, 8): Node((0, 8), (1, 7), 6.242640687119285, 0), (2, 10): Node((2, 10), (2, 9), 6.242640687119285, 0), (10, 2): Node((10, 2), (9, 2), 6.242640687119285, 0), (2, 0): Node((2, 0), (3, 1), 6.242640687119285, 0), (6, 11): Node((6, 11), (6, 10), 6.414213562373095, 0), (11, 6): Node((11, 6), (10, 6), 6.414213562373095, 0), (11, 4): Node((11, 4), (10, 4), 6.414213562373095, 0), (4, 11): Node((4, 11), (4, 10), 6.414213562373095, 0), (1, 10): Node((1, 10), (2, 9), 6.65685424949238, 0), (0, 1): Node((0, 1), (1, 2), 6.65685424949238, 0), (10, 1): Node((10, 1), (9, 2), 6.65685424949238, 0), (9, 0): Node((9, 0), (8, 1), 6.65685424949238, 0), (0, 9): Node((0, 9), (1, 8), 6.65685424949238, 0), (10, 9): Node((10, 9), (9, 8), 6.65685424949238, 0), (1, 0): Node((1, 0), (2, 1), 6.65685424949238, 0), (9, 10): Node((9, 10), (8, 9), 6.65685424949238, 0), (7, 11): Node((7, 11), (6, 10), 6.82842712474619, 0), (11, 7): Node((11, 7), (10, 6), 6.82842712474619, 0), (11, 3): Node((11, 3), (10, 4), 6.82842712474619, 0), (3, 11): Node((3, 11), (4, 10), 6.82842712474619, 0), (5, 12): Node((5, 12), (5, 11), 7.0, 0), (12, 5): Node((12, 5), (11, 5), 7.0, 0), (0, 10): Node((0, 10), (1, 9), 7.0710678118654755, 0), (10, 0): Node((10, 0), (9, 1), 7.0710678118654755, 0), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0)}})
Python Console Session
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
>>> planner.plan()
([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 0), (4, 5): Node((4, 5), (5, 5), 1.0, 0), (6, 5): Node((6, 5), (5, 5), 1.0, 0), (5, 4): Node((5, 4), (5, 5), 1.0, 0), (5, 6): Node((5, 6), (5, 5), 1.0, 0), (4, 6): Node((4, 6), (5, 5), 1.4142135623730951, 0), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 0), (6, 4): Node((6, 4), (5, 5), 1.4142135623730951, 0), (4, 4): Node((4, 4), (5, 5), 1.4142135623730951, 0), (5, 7): Node((5, 7), (5, 6), 2.0, 0), (5, 3): Node((5, 3), (5, 4), 2.0, 0), (3, 5): Node((3, 5), (4, 5), 2.0, 0), (7, 5): Node((7, 5), (6, 5), 2.0, 0), (6, 7): Node((6, 7), (6, 6), 2.414213562373095, 0), (4, 7): Node((4, 7), (4, 6), 2.414213562373095, 0), (4, 3): Node((4, 3), (4, 4), 2.414213562373095, 0), (6, 3): Node((6, 3), (5, 4), 2.414213562373095, 0), (3, 4): Node((3, 4), (4, 4), 2.414213562373095, 0), (3, 6): Node((3, 6), (4, 5), 2.414213562373095, 0), (7, 6): Node((7, 6), (6, 5), 2.414213562373095, 0), (7, 4): Node((7, 4), (6, 4), 2.414213562373095, 0), (3, 7): Node((3, 7), (4, 6), 2.8284271247461903, 0), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 0), (3, 3): Node((3, 3), (4, 4), 2.8284271247461903, 0), (7, 3): Node((7, 3), (6, 4), 2.8284271247461903, 0), (8, 5): Node((8, 5), (7, 5), 3.0, 0), (2, 5): Node((2, 5), (3, 5), 3.0, 0), (5, 2): Node((5, 2), (5, 3), 3.0, 0), (5, 8): Node((5, 8), (5, 7), 3.0, 0), (8, 4): Node((8, 4), (7, 5), 3.414213562373095, 0), (2, 4): Node((2, 4), (3, 4), 3.414213562373095, 0), (8, 6): Node((8, 6), (7, 5), 3.414213562373095, 0), (6, 2): Node((6, 2), (6, 3), 3.414213562373095, 0), (4, 8): Node((4, 8), (4, 7), 3.414213562373095, 0), (6, 8): Node((6, 8), (6, 7), 3.414213562373095, 0), (2, 6): Node((2, 6), (3, 5), 3.414213562373095, 0), (4, 2): Node((4, 2), (4, 3), 3.414213562373095, 0), (7, 8): Node((7, 8), (6, 7), 3.82842712474619, 0), (2, 3): Node((2, 3), (3, 4), 3.82842712474619, 0), (8, 7): Node((8, 7), (7, 6), 3.82842712474619, 0), (7, 2): Node((7, 2), (6, 3), 3.82842712474619, 0), (3, 2): Node((3, 2), (4, 3), 3.82842712474619, 0), (3, 8): Node((3, 8), (4, 7), 3.82842712474619, 0), (2, 7): Node((2, 7), (3, 6), 3.82842712474619, 0), (8, 3): Node((8, 3), (7, 4), 3.82842712474619, 0), (5, 1): Node((5, 1), (5, 2), 4.0, 0), (5, 9): Node((5, 9), (5, 8), 4.0, 0), (9, 5): Node((9, 5), (8, 5), 4.0, 0), (1, 5): Node((1, 5), (2, 5), 4.0, 0), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 0), (8, 2): Node((8, 2), (7, 3), 4.242640687119286, 0), (2, 2): Node((2, 2), (3, 3), 4.242640687119286, 0), (2, 8): Node((2, 8), (3, 7), 4.242640687119286, 0), (9, 4): Node((9, 4), (8, 4), 4.414213562373095, 0), (6, 1): Node((6, 1), (6, 2), 4.414213562373095, 0), (6, 9): Node((6, 9), (5, 8), 4.414213562373095, 0), (4, 9): Node((4, 9), (4, 8), 4.414213562373095, 0), (9, 6): Node((9, 6), (8, 5), 4.414213562373095, 0), (1, 4): Node((1, 4), (2, 4), 4.414213562373095, 0), (4, 1): Node((4, 1), (5, 2), 4.414213562373095, 0), (1, 6): Node((1, 6), (2, 5), 4.414213562373095, 0), (7, 1): Node((7, 1), (6, 2), 4.82842712474619, 0), (3, 9): Node((3, 9), (3, 8), 4.82842712474619, 0), (9, 3): Node((9, 3), (8, 3), 4.82842712474619, 0), (9, 7): Node((9, 7), (8, 6), 4.82842712474619, 0), (1, 3): Node((1, 3), (2, 3), 4.82842712474619, 0), (7, 9): Node((7, 9), (7, 8), 4.82842712474619, 0), (1, 7): Node((1, 7), (2, 6), 4.82842712474619, 0), (3, 1): Node((3, 1), (3, 2), 4.82842712474619, 0), (5, 10): Node((5, 10), (5, 9), 5.0, 0), (5, 0): Node((5, 0), (5, 1), 5.0, 0), (0, 5): Node((0, 5), (1, 5), 5.0, 0), (10, 5): Node((10, 5), (9, 5), 5.0, 0), (9, 2): Node((9, 2), (8, 3), 5.242640687119285, 0), (9, 8): Node((9, 8), (8, 7), 5.242640687119285, 0), (8, 9): Node((8, 9), (7, 8), 5.242640687119285, 0), (1, 2): Node((1, 2), (2, 3), 5.242640687119285, 0), (2, 9): Node((2, 9), (3, 8), 5.242640687119285, 0), (8, 1): Node((8, 1), (7, 2), 5.242640687119285, 0), (2, 1): Node((2, 1), (3, 2), 5.242640687119285, 0), (1, 8): Node((1, 8), (2, 7), 5.242640687119285, 0), (0, 6): Node((0, 6), (1, 5), 5.414213562373095, 0), (4, 0): Node((4, 0), (4, 1), 5.414213562373095, 0), (6, 10): Node((6, 10), (6, 9), 5.414213562373095, 0), (6, 0): Node((6, 0), (6, 1), 5.414213562373095, 0), (10, 4): Node((10, 4), (9, 4), 5.414213562373095, 0), (4, 10): Node((4, 10), (5, 9), 5.414213562373095, 0), (0, 4): Node((0, 4), (1, 5), 5.414213562373095, 0), (10, 6): Node((10, 6), (9, 6), 5.414213562373095, 0), (1, 9): Node((1, 9), (2, 8), 5.656854249492381, 0), (1, 1): Node((1, 1), (2, 2), 5.656854249492381, 0), (9, 1): Node((9, 1), (8, 2), 5.656854249492381, 0), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 0), (7, 10): Node((7, 10), (6, 9), 5.82842712474619, 0), (7, 0): Node((7, 0), (6, 1), 5.82842712474619, 0), (10, 3): Node((10, 3), (9, 4), 5.82842712474619, 0), (3, 0): Node((3, 0), (4, 1), 5.82842712474619, 0), (3, 10): Node((3, 10), (3, 9), 5.82842712474619, 0), (10, 7): Node((10, 7), (9, 7), 5.82842712474619, 0), (0, 3): Node((0, 3), (1, 4), 5.82842712474619, 0), (0, 7): Node((0, 7), (1, 7), 5.82842712474619, 0), (11, 5): Node((11, 5), (10, 5), 6.0, 0), (5, 11): Node((5, 11), (5, 10), 6.0, 0), (0, 8): Node((0, 8), (1, 8), 6.242640687119285, 0), (8, 10): Node((8, 10), (8, 9), 6.242640687119285, 0), (10, 2): Node((10, 2), (9, 3), 6.242640687119285, 0), (8, 0): Node((8, 0), (8, 1), 6.242640687119285, 0), (2, 10): Node((2, 10), (3, 9), 6.242640687119285, 0), (10, 8): Node((10, 8), (9, 7), 6.242640687119285, 0), (2, 0): Node((2, 0), (3, 1), 6.242640687119285, 0), (0, 2): Node((0, 2), (1, 2), 6.242640687119285, 0), (11, 4): Node((11, 4), (10, 5), 6.414213562373095, 0), (4, 11): Node((4, 11), (5, 10), 6.414213562373095, 0), (11, 6): Node((11, 6), (10, 5), 6.414213562373095, 0), (6, 11): Node((6, 11), (6, 10), 6.414213562373095, 0), (0, 1): Node((0, 1), (1, 2), 6.65685424949238, 0), (9, 0): Node((9, 0), (8, 1), 6.65685424949238, 0), (0, 9): Node((0, 9), (1, 8), 6.65685424949238, 0), (1, 0): Node((1, 0), (2, 1), 6.65685424949238, 0), (9, 10): Node((9, 10), (8, 9), 6.65685424949238, 0), (10, 9): Node((10, 9), (9, 8), 6.65685424949238, 0), (10, 1): Node((10, 1), (9, 2), 6.65685424949238, 0), (1, 10): Node((1, 10), (2, 9), 6.65685424949238, 0), (7, 11): Node((7, 11), (6, 10), 6.82842712474619, 0), (11, 3): Node((11, 3), (10, 3), 6.82842712474619, 0), (3, 11): Node((3, 11), (3, 10), 6.82842712474619, 0), (11, 7): Node((11, 7), (10, 7), 6.82842712474619, 0), (5, 12): Node((5, 12), (5, 11), 7.0, 0), (12, 5): Node((12, 5), (11, 5), 7.0, 0), (0, 10): Node((0, 10), (1, 9), 7.0710678118654755, 0), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0)}})
Source code in src\python_motion_planning\path_planner\graph_search\dijkstra.py
Python
class Dijkstra(BasePathPlanner):
    """
    Class for Dijkstra path planner.

    Args:
        *args: see the parent class.
        *kwargs: see the parent class.

    References:
        [1] A Note on Two Problems in Connexion with Graphs

    Examples:
        >>> map_ = Grid(bounds=[[0, 15], [0, 15]])
        >>> planner = Dijkstra(map_=map_, start=(5, 5), goal=(10, 10))
        >>> planner.plan()
        ([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 0), (4, 5): Node((4, 5), (5, 5), 1.0, 0), (6, 5): Node((6, 5), (5, 5), 1.0, 0), (5, 4): Node((5, 4), (5, 5), 1.0, 0), (5, 6): Node((5, 6), (5, 5), 1.0, 0), (4, 6): Node((4, 6), (5, 5), 1.4142135623730951, 0), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 0), (6, 4): Node((6, 4), (5, 5), 1.4142135623730951, 0), (4, 4): Node((4, 4), (5, 5), 1.4142135623730951, 0), (5, 7): Node((5, 7), (5, 6), 2.0, 0), (5, 3): Node((5, 3), (5, 4), 2.0, 0), (3, 5): Node((3, 5), (4, 5), 2.0, 0), (7, 5): Node((7, 5), (6, 5), 2.0, 0), (4, 7): Node((4, 7), (4, 6), 2.414213562373095, 0), (3, 6): Node((3, 6), (4, 6), 2.414213562373095, 0), (6, 7): Node((6, 7), (5, 6), 2.414213562373095, 0), (4, 3): Node((4, 3), (4, 4), 2.414213562373095, 0), (6, 3): Node((6, 3), (5, 4), 2.414213562373095, 0), (3, 4): Node((3, 4), (4, 4), 2.414213562373095, 0), (7, 6): Node((7, 6), (6, 5), 2.414213562373095, 0), (7, 4): Node((7, 4), (6, 4), 2.414213562373095, 0), (3, 7): Node((3, 7), (4, 6), 2.8284271247461903, 0), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 0), (3, 3): Node((3, 3), (4, 4), 2.8284271247461903, 0), (7, 3): Node((7, 3), (6, 4), 2.8284271247461903, 0), (8, 5): Node((8, 5), (7, 5), 3.0, 0), (2, 5): Node((2, 5), (3, 5), 3.0, 0), (5, 2): Node((5, 2), (5, 3), 3.0, 0), (5, 8): Node((5, 8), (5, 7), 3.0, 0), (4, 2): Node((4, 2), (4, 3), 3.414213562373095, 0), (6, 8): Node((6, 8), (5, 7), 3.414213562373095, 0), (8, 6): Node((8, 6), (7, 6), 3.414213562373095, 0), (8, 4): Node((8, 4), (7, 5), 3.414213562373095, 0), (4, 8): Node((4, 8), (4, 7), 3.414213562373095, 0), (2, 6): Node((2, 6), (3, 6), 3.414213562373095, 0), (2, 4): Node((2, 4), (3, 5), 3.414213562373095, 0), (6, 2): Node((6, 2), (6, 3), 3.414213562373095, 0), (3, 2): Node((3, 2), (4, 3), 3.82842712474619, 0), (2, 7): Node((2, 7), (3, 6), 3.82842712474619, 0), (3, 8): Node((3, 8), (4, 7), 3.82842712474619, 0), (7, 8): Node((7, 8), (6, 7), 3.82842712474619, 0), (8, 7): Node((8, 7), (7, 6), 3.82842712474619, 0), (7, 2): Node((7, 2), (6, 3), 3.82842712474619, 0), (8, 3): Node((8, 3), (7, 4), 3.82842712474619, 0), (2, 3): Node((2, 3), (3, 4), 3.82842712474619, 0), (9, 5): Node((9, 5), (8, 5), 4.0, 0), (5, 9): Node((5, 9), (5, 8), 4.0, 0), (5, 1): Node((5, 1), (5, 2), 4.0, 0), (1, 5): Node((1, 5), (2, 5), 4.0, 0), (8, 2): Node((8, 2), (7, 3), 4.242640687119286, 0), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 0), (2, 8): Node((2, 8), (3, 7), 4.242640687119286, 0), (2, 2): Node((2, 2), (3, 3), 4.242640687119286, 0), (9, 4): Node((9, 4), (8, 5), 4.414213562373095, 0), (9, 6): Node((9, 6), (8, 5), 4.414213562373095, 0), (4, 1): Node((4, 1), (4, 2), 4.414213562373095, 0), (1, 6): Node((1, 6), (2, 6), 4.414213562373095, 0), (4, 9): Node((4, 9), (5, 8), 4.414213562373095, 0), (6, 9): Node((6, 9), (6, 8), 4.414213562373095, 0), (6, 1): Node((6, 1), (5, 2), 4.414213562373095, 0), (1, 4): Node((1, 4), (2, 5), 4.414213562373095, 0), (1, 7): Node((1, 7), (2, 6), 4.82842712474619, 0), (7, 1): Node((7, 1), (7, 2), 4.82842712474619, 0), (7, 9): Node((7, 9), (6, 8), 4.82842712474619, 0), (9, 3): Node((9, 3), (8, 4), 4.82842712474619, 0), (3, 1): Node((3, 1), (3, 2), 4.82842712474619, 0), (9, 7): Node((9, 7), (8, 6), 4.82842712474619, 0), (1, 3): Node((1, 3), (2, 3), 4.82842712474619, 0), (3, 9): Node((3, 9), (3, 8), 4.82842712474619, 0), (5, 10): Node((5, 10), (5, 9), 5.0, 0), (0, 5): Node((0, 5), (1, 5), 5.0, 0), (10, 5): Node((10, 5), (9, 5), 5.0, 0), (5, 0): Node((5, 0), (5, 1), 5.0, 0), (2, 9): Node((2, 9), (3, 8), 5.242640687119285, 0), (8, 1): Node((8, 1), (7, 2), 5.242640687119285, 0), (1, 8): Node((1, 8), (2, 7), 5.242640687119285, 0), (2, 1): Node((2, 1), (3, 2), 5.242640687119285, 0), (8, 9): Node((8, 9), (7, 8), 5.242640687119285, 0), (9, 8): Node((9, 8), (8, 7), 5.242640687119285, 0), (9, 2): Node((9, 2), (8, 3), 5.242640687119285, 0), (1, 2): Node((1, 2), (2, 3), 5.242640687119285, 0), (0, 6): Node((0, 6), (1, 6), 5.414213562373095, 0), (4, 0): Node((4, 0), (4, 1), 5.414213562373095, 0), (10, 6): Node((10, 6), (9, 6), 5.414213562373095, 0), (10, 4): Node((10, 4), (9, 4), 5.414213562373095, 0), (0, 4): Node((0, 4), (1, 5), 5.414213562373095, 0), (4, 10): Node((4, 10), (5, 9), 5.414213562373095, 0), (6, 10): Node((6, 10), (5, 9), 5.414213562373095, 0), (6, 0): Node((6, 0), (5, 1), 5.414213562373095, 0), (1, 1): Node((1, 1), (2, 2), 5.656854249492381, 0), (9, 1): Node((9, 1), (8, 2), 5.656854249492381, 0), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 0), (1, 9): Node((1, 9), (2, 8), 5.656854249492381, 0), (0, 3): Node((0, 3), (1, 4), 5.82842712474619, 0), (3, 0): Node((3, 0), (4, 1), 5.82842712474619, 0), (10, 3): Node((10, 3), (9, 4), 5.82842712474619, 0), (10, 7): Node((10, 7), (9, 6), 5.82842712474619, 0), (7, 0): Node((7, 0), (7, 1), 5.82842712474619, 0), (0, 7): Node((0, 7), (1, 7), 5.82842712474619, 0), (3, 10): Node((3, 10), (3, 9), 5.82842712474619, 0), (7, 10): Node((7, 10), (6, 9), 5.82842712474619, 0), (5, 11): Node((5, 11), (5, 10), 6.0, 0), (11, 5): Node((11, 5), (10, 5), 6.0, 0), (8, 0): Node((8, 0), (8, 1), 6.242640687119285, 0), (10, 8): Node((10, 8), (9, 8), 6.242640687119285, 0), (0, 2): Node((0, 2), (1, 3), 6.242640687119285, 0), (8, 10): Node((8, 10), (7, 9), 6.242640687119285, 0), (0, 8): Node((0, 8), (1, 7), 6.242640687119285, 0), (2, 10): Node((2, 10), (2, 9), 6.242640687119285, 0), (10, 2): Node((10, 2), (9, 2), 6.242640687119285, 0), (2, 0): Node((2, 0), (3, 1), 6.242640687119285, 0), (6, 11): Node((6, 11), (6, 10), 6.414213562373095, 0), (11, 6): Node((11, 6), (10, 6), 6.414213562373095, 0), (11, 4): Node((11, 4), (10, 4), 6.414213562373095, 0), (4, 11): Node((4, 11), (4, 10), 6.414213562373095, 0), (1, 10): Node((1, 10), (2, 9), 6.65685424949238, 0), (0, 1): Node((0, 1), (1, 2), 6.65685424949238, 0), (10, 1): Node((10, 1), (9, 2), 6.65685424949238, 0), (9, 0): Node((9, 0), (8, 1), 6.65685424949238, 0), (0, 9): Node((0, 9), (1, 8), 6.65685424949238, 0), (10, 9): Node((10, 9), (9, 8), 6.65685424949238, 0), (1, 0): Node((1, 0), (2, 1), 6.65685424949238, 0), (9, 10): Node((9, 10), (8, 9), 6.65685424949238, 0), (7, 11): Node((7, 11), (6, 10), 6.82842712474619, 0), (11, 7): Node((11, 7), (10, 6), 6.82842712474619, 0), (11, 3): Node((11, 3), (10, 4), 6.82842712474619, 0), (3, 11): Node((3, 11), (4, 10), 6.82842712474619, 0), (5, 12): Node((5, 12), (5, 11), 7.0, 0), (12, 5): Node((12, 5), (11, 5), 7.0, 0), (0, 10): Node((0, 10), (1, 9), 7.0710678118654755, 0), (10, 0): Node((10, 0), (9, 1), 7.0710678118654755, 0), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0)}})

        >>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
        >>> planner.plan()
        ([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 0), (4, 5): Node((4, 5), (5, 5), 1.0, 0), (6, 5): Node((6, 5), (5, 5), 1.0, 0), (5, 4): Node((5, 4), (5, 5), 1.0, 0), (5, 6): Node((5, 6), (5, 5), 1.0, 0), (4, 6): Node((4, 6), (5, 5), 1.4142135623730951, 0), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 0), (6, 4): Node((6, 4), (5, 5), 1.4142135623730951, 0), (4, 4): Node((4, 4), (5, 5), 1.4142135623730951, 0), (5, 7): Node((5, 7), (5, 6), 2.0, 0), (5, 3): Node((5, 3), (5, 4), 2.0, 0), (3, 5): Node((3, 5), (4, 5), 2.0, 0), (7, 5): Node((7, 5), (6, 5), 2.0, 0), (6, 7): Node((6, 7), (6, 6), 2.414213562373095, 0), (4, 7): Node((4, 7), (4, 6), 2.414213562373095, 0), (4, 3): Node((4, 3), (4, 4), 2.414213562373095, 0), (6, 3): Node((6, 3), (5, 4), 2.414213562373095, 0), (3, 4): Node((3, 4), (4, 4), 2.414213562373095, 0), (3, 6): Node((3, 6), (4, 5), 2.414213562373095, 0), (7, 6): Node((7, 6), (6, 5), 2.414213562373095, 0), (7, 4): Node((7, 4), (6, 4), 2.414213562373095, 0), (3, 7): Node((3, 7), (4, 6), 2.8284271247461903, 0), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 0), (3, 3): Node((3, 3), (4, 4), 2.8284271247461903, 0), (7, 3): Node((7, 3), (6, 4), 2.8284271247461903, 0), (8, 5): Node((8, 5), (7, 5), 3.0, 0), (2, 5): Node((2, 5), (3, 5), 3.0, 0), (5, 2): Node((5, 2), (5, 3), 3.0, 0), (5, 8): Node((5, 8), (5, 7), 3.0, 0), (8, 4): Node((8, 4), (7, 5), 3.414213562373095, 0), (2, 4): Node((2, 4), (3, 4), 3.414213562373095, 0), (8, 6): Node((8, 6), (7, 5), 3.414213562373095, 0), (6, 2): Node((6, 2), (6, 3), 3.414213562373095, 0), (4, 8): Node((4, 8), (4, 7), 3.414213562373095, 0), (6, 8): Node((6, 8), (6, 7), 3.414213562373095, 0), (2, 6): Node((2, 6), (3, 5), 3.414213562373095, 0), (4, 2): Node((4, 2), (4, 3), 3.414213562373095, 0), (7, 8): Node((7, 8), (6, 7), 3.82842712474619, 0), (2, 3): Node((2, 3), (3, 4), 3.82842712474619, 0), (8, 7): Node((8, 7), (7, 6), 3.82842712474619, 0), (7, 2): Node((7, 2), (6, 3), 3.82842712474619, 0), (3, 2): Node((3, 2), (4, 3), 3.82842712474619, 0), (3, 8): Node((3, 8), (4, 7), 3.82842712474619, 0), (2, 7): Node((2, 7), (3, 6), 3.82842712474619, 0), (8, 3): Node((8, 3), (7, 4), 3.82842712474619, 0), (5, 1): Node((5, 1), (5, 2), 4.0, 0), (5, 9): Node((5, 9), (5, 8), 4.0, 0), (9, 5): Node((9, 5), (8, 5), 4.0, 0), (1, 5): Node((1, 5), (2, 5), 4.0, 0), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 0), (8, 2): Node((8, 2), (7, 3), 4.242640687119286, 0), (2, 2): Node((2, 2), (3, 3), 4.242640687119286, 0), (2, 8): Node((2, 8), (3, 7), 4.242640687119286, 0), (9, 4): Node((9, 4), (8, 4), 4.414213562373095, 0), (6, 1): Node((6, 1), (6, 2), 4.414213562373095, 0), (6, 9): Node((6, 9), (5, 8), 4.414213562373095, 0), (4, 9): Node((4, 9), (4, 8), 4.414213562373095, 0), (9, 6): Node((9, 6), (8, 5), 4.414213562373095, 0), (1, 4): Node((1, 4), (2, 4), 4.414213562373095, 0), (4, 1): Node((4, 1), (5, 2), 4.414213562373095, 0), (1, 6): Node((1, 6), (2, 5), 4.414213562373095, 0), (7, 1): Node((7, 1), (6, 2), 4.82842712474619, 0), (3, 9): Node((3, 9), (3, 8), 4.82842712474619, 0), (9, 3): Node((9, 3), (8, 3), 4.82842712474619, 0), (9, 7): Node((9, 7), (8, 6), 4.82842712474619, 0), (1, 3): Node((1, 3), (2, 3), 4.82842712474619, 0), (7, 9): Node((7, 9), (7, 8), 4.82842712474619, 0), (1, 7): Node((1, 7), (2, 6), 4.82842712474619, 0), (3, 1): Node((3, 1), (3, 2), 4.82842712474619, 0), (5, 10): Node((5, 10), (5, 9), 5.0, 0), (5, 0): Node((5, 0), (5, 1), 5.0, 0), (0, 5): Node((0, 5), (1, 5), 5.0, 0), (10, 5): Node((10, 5), (9, 5), 5.0, 0), (9, 2): Node((9, 2), (8, 3), 5.242640687119285, 0), (9, 8): Node((9, 8), (8, 7), 5.242640687119285, 0), (8, 9): Node((8, 9), (7, 8), 5.242640687119285, 0), (1, 2): Node((1, 2), (2, 3), 5.242640687119285, 0), (2, 9): Node((2, 9), (3, 8), 5.242640687119285, 0), (8, 1): Node((8, 1), (7, 2), 5.242640687119285, 0), (2, 1): Node((2, 1), (3, 2), 5.242640687119285, 0), (1, 8): Node((1, 8), (2, 7), 5.242640687119285, 0), (0, 6): Node((0, 6), (1, 5), 5.414213562373095, 0), (4, 0): Node((4, 0), (4, 1), 5.414213562373095, 0), (6, 10): Node((6, 10), (6, 9), 5.414213562373095, 0), (6, 0): Node((6, 0), (6, 1), 5.414213562373095, 0), (10, 4): Node((10, 4), (9, 4), 5.414213562373095, 0), (4, 10): Node((4, 10), (5, 9), 5.414213562373095, 0), (0, 4): Node((0, 4), (1, 5), 5.414213562373095, 0), (10, 6): Node((10, 6), (9, 6), 5.414213562373095, 0), (1, 9): Node((1, 9), (2, 8), 5.656854249492381, 0), (1, 1): Node((1, 1), (2, 2), 5.656854249492381, 0), (9, 1): Node((9, 1), (8, 2), 5.656854249492381, 0), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 0), (7, 10): Node((7, 10), (6, 9), 5.82842712474619, 0), (7, 0): Node((7, 0), (6, 1), 5.82842712474619, 0), (10, 3): Node((10, 3), (9, 4), 5.82842712474619, 0), (3, 0): Node((3, 0), (4, 1), 5.82842712474619, 0), (3, 10): Node((3, 10), (3, 9), 5.82842712474619, 0), (10, 7): Node((10, 7), (9, 7), 5.82842712474619, 0), (0, 3): Node((0, 3), (1, 4), 5.82842712474619, 0), (0, 7): Node((0, 7), (1, 7), 5.82842712474619, 0), (11, 5): Node((11, 5), (10, 5), 6.0, 0), (5, 11): Node((5, 11), (5, 10), 6.0, 0), (0, 8): Node((0, 8), (1, 8), 6.242640687119285, 0), (8, 10): Node((8, 10), (8, 9), 6.242640687119285, 0), (10, 2): Node((10, 2), (9, 3), 6.242640687119285, 0), (8, 0): Node((8, 0), (8, 1), 6.242640687119285, 0), (2, 10): Node((2, 10), (3, 9), 6.242640687119285, 0), (10, 8): Node((10, 8), (9, 7), 6.242640687119285, 0), (2, 0): Node((2, 0), (3, 1), 6.242640687119285, 0), (0, 2): Node((0, 2), (1, 2), 6.242640687119285, 0), (11, 4): Node((11, 4), (10, 5), 6.414213562373095, 0), (4, 11): Node((4, 11), (5, 10), 6.414213562373095, 0), (11, 6): Node((11, 6), (10, 5), 6.414213562373095, 0), (6, 11): Node((6, 11), (6, 10), 6.414213562373095, 0), (0, 1): Node((0, 1), (1, 2), 6.65685424949238, 0), (9, 0): Node((9, 0), (8, 1), 6.65685424949238, 0), (0, 9): Node((0, 9), (1, 8), 6.65685424949238, 0), (1, 0): Node((1, 0), (2, 1), 6.65685424949238, 0), (9, 10): Node((9, 10), (8, 9), 6.65685424949238, 0), (10, 9): Node((10, 9), (9, 8), 6.65685424949238, 0), (10, 1): Node((10, 1), (9, 2), 6.65685424949238, 0), (1, 10): Node((1, 10), (2, 9), 6.65685424949238, 0), (7, 11): Node((7, 11), (6, 10), 6.82842712474619, 0), (11, 3): Node((11, 3), (10, 3), 6.82842712474619, 0), (3, 11): Node((3, 11), (3, 10), 6.82842712474619, 0), (11, 7): Node((11, 7), (10, 7), 6.82842712474619, 0), (5, 12): Node((5, 12), (5, 11), 7.0, 0), (12, 5): Node((12, 5), (11, 5), 7.0, 0), (0, 10): Node((0, 10), (1, 9), 7.0710678118654755, 0), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0)}})
    """
    def __init__(self, *args, **kwargs) -> None:
        super().__init__(*args, **kwargs)

    def __str__(self) -> str:
        return "Dijkstra"

    def plan(self) -> Union[list, dict]:
        """
        Interface for planning.

        Returns:
            path: A list containing the path waypoints
            path_info: A dictionary containing the path information (success, length, cost, expand)
        """
        # OPEN list (priority queue) and CLOSED list (hash table)
        OPEN = []
        # For Dijkstra, we only use g-value (no heuristic h-value)
        start_node = Node(self.start, None, 0, 0)
        heapq.heappush(OPEN, start_node)
        CLOSED = dict()

        while OPEN:
            node = heapq.heappop(OPEN)

            # obstacle found
            if not self.map_.is_expandable(node.current, node.parent):
                continue

            # exists in CLOSED list
            if node.current in CLOSED:
                continue

            # goal found
            if node.current == self.goal:
                CLOSED[node.current] = node
                path, length, cost = self.extract_path(CLOSED)
                return path, {
                    "success": True, 
                    "start": self.start, 
                    "goal": self.goal, 
                    "length": length, 
                    "cost": cost, 
                    "expand": CLOSED
                }

            for node_n in self.map_.get_neighbors(node): 
                # exists in CLOSED list
                if node_n.current in CLOSED:
                    continue

                # For Dijkstra, we only update g-value (no heuristic)
                node_n.g = node.g + self.get_cost(node.current, node_n.current)

                # goal found
                if node_n.current == self.goal:
                    heapq.heappush(OPEN, node_n)
                    break

                # update OPEN list with node sorted by g-value
                heapq.heappush(OPEN, node_n)

            CLOSED[node.current] = node

        self.failed_info[1]["expand"] = CLOSED
        return self.failed_info

plan()

Interface for planning.

Returns:

Name Type Description
path Union[list, dict]

A list containing the path waypoints
path_info Union[list, dict]

A dictionary containing the path information (success, length, cost, expand)
Source code in src\python_motion_planning\path_planner\graph_search\dijkstra.py
Python
def plan(self) -> Union[list, dict]:
    """
    Interface for planning.

    Returns:
        path: A list containing the path waypoints
        path_info: A dictionary containing the path information (success, length, cost, expand)
    """
    # OPEN list (priority queue) and CLOSED list (hash table)
    OPEN = []
    # For Dijkstra, we only use g-value (no heuristic h-value)
    start_node = Node(self.start, None, 0, 0)
    heapq.heappush(OPEN, start_node)
    CLOSED = dict()

    while OPEN:
        node = heapq.heappop(OPEN)

        # obstacle found
        if not self.map_.is_expandable(node.current, node.parent):
            continue

        # exists in CLOSED list
        if node.current in CLOSED:
            continue

        # goal found
        if node.current == self.goal:
            CLOSED[node.current] = node
            path, length, cost = self.extract_path(CLOSED)
            return path, {
                "success": True, 
                "start": self.start, 
                "goal": self.goal, 
                "length": length, 
                "cost": cost, 
                "expand": CLOSED
            }

        for node_n in self.map_.get_neighbors(node): 
            # exists in CLOSED list
            if node_n.current in CLOSED:
                continue

            # For Dijkstra, we only update g-value (no heuristic)
            node_n.g = node.g + self.get_cost(node.current, node_n.current)

            # goal found
            if node_n.current == self.goal:
                heapq.heappush(OPEN, node_n)
                break

            # update OPEN list with node sorted by g-value
            heapq.heappush(OPEN, node_n)

        CLOSED[node.current] = node

    self.failed_info[1]["expand"] = CLOSED
    return self.failed_info