Skip to content

Commit c70b7be

Browse files
omigeftomigeft
committed
v2.0.dev2
1 parent f31be81 commit c70b7be

File tree

9 files changed

+56
-28
lines changed

9 files changed

+56
-28
lines changed

‎src/python_motion_planning/path_planner/graph_search/a_star.py‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -23,12 +23,14 @@ class AStar(Dijkstra):
2323
Examples:
2424
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
2525
>>> planner = AStar(map_=map_, start=(5, 5), goal=(10, 10))
26-
>>> planner.plan()
27-
([(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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
28-
26+
>>> path, path_info = planner.plan()
27+
>>> print(path_info['success'])
28+
True
29+
2930
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
30-
>>> planner.plan()
31-
([(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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
31+
>>> path, path_info = planner.plan()
32+
>>> print(path_info['success'])
33+
True
3234
"""
3335
def __init__(self, *args, **kwargs) -> None:
3436
super().__init__(*args, **kwargs)

‎src/python_motion_planning/path_planner/graph_search/dijkstra.py‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -24,12 +24,14 @@ class Dijkstra(BasePathPlanner):
2424
Examples:
2525
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
2626
>>> planner = Dijkstra(map_=map_, start=(5, 5), goal=(10, 10))
27-
>>> planner.plan()
28-
([(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)}})
29-
27+
>>> path, path_info = planner.plan()
28+
>>> print(path_info['success'])
29+
True
30+
3031
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
31-
>>> planner.plan()
32-
([(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)}})
32+
>>> path, path_info = planner.plan()
33+
>>> print(path_info['success'])
34+
True
3335
"""
3436
def __init__(self, *args, diagonal: bool = True, **kwargs) -> None:
3537
super().__init__(*args, **kwargs)

‎src/python_motion_planning/path_planner/graph_search/gbfs.py‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,12 +23,14 @@ class GBFS(Dijkstra):
2323
Examples:
2424
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
2525
>>> planner = GBFS(map_=map_, start=(5, 5), goal=(10, 10))
26-
>>> planner.plan()
27-
([(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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
26+
>>> path, path_info = planner.plan()
27+
>>> print(path_info['success'])
28+
True
2829
2930
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
30-
>>> planner.plan()
31-
([(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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
31+
>>> path, path_info = planner.plan()
32+
>>> print(path_info['success'])
33+
True
3234
"""
3335
def __init__(self, *args, **kwargs) -> None:
3436
super().__init__(*args, **kwargs)

‎src/python_motion_planning/path_planner/graph_search/jps.py‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,14 @@ class JPS(AStar):
3131
Examples:
3232
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
3333
>>> planner = JPS(map_=map_, start=(5, 5), goal=(10, 10))
34-
>>> planner.plan()
34+
>>> path, path_info = planner.plan()
35+
>>> print(path_info['success'])
36+
True
37+
38+
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
39+
>>> path, path_info = planner.plan()
40+
>>> print(path_info['success'])
41+
True
3542
"""
3643
def __init__(self, *args, **kwargs) -> None:
3744
super().__init__(*args, **kwargs)

‎src/python_motion_planning/path_planner/graph_search/lazy_theta_star.py‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -24,12 +24,14 @@ class LazyThetaStar(ThetaStar):
2424
Examples:
2525
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
2626
>>> planner = LazyThetaStar(map_=map_, start=(5, 5), goal=(10, 10))
27-
>>> planner.plan()
28-
([(5, 5), (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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (5, 5), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (5, 5), 4.242640687119285, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.65685424949238, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.071067811865475, 0.0)}})
29-
27+
>>> path, path_info = planner.plan()
28+
>>> print(path_info['success'])
29+
True
30+
3031
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
31-
>>> planner.plan()
32-
([(5, 5), (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, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (5, 5), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (5, 5), 4.242640687119285, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.65685424949238, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.071067811865475, 0.0)}})
32+
>>> path, path_info = planner.plan()
33+
>>> print(path_info['success'])
34+
True
3335
"""
3436
def __init__(self, *args, **kwargs) -> None:
3537
super().__init__(*args, **kwargs)

0 commit comments

Comments
 (0)