python-motion-planning 2.0.dev1__py3-none-any.whl → 2.0.dev2__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- python_motion_planning/common/env/map/base_map.py +2 -8
- python_motion_planning/common/env/map/grid.py +74 -81
- python_motion_planning/common/utils/__init__.py +2 -1
- python_motion_planning/common/utils/child_tree.py +22 -0
- python_motion_planning/common/visualizer/__init__.py +3 -1
- python_motion_planning/common/visualizer/base_visualizer.py +165 -0
- python_motion_planning/common/visualizer/{visualizer.py → visualizer_2d.py} +97 -220
- python_motion_planning/common/visualizer/visualizer_3d.py +242 -0
- python_motion_planning/controller/base_controller.py +37 -4
- python_motion_planning/controller/path_tracker/__init__.py +2 -1
- python_motion_planning/controller/path_tracker/apf.py +22 -23
- python_motion_planning/controller/path_tracker/dwa.py +14 -17
- python_motion_planning/controller/path_tracker/path_tracker.py +4 -1
- python_motion_planning/controller/path_tracker/pid.py +7 -1
- python_motion_planning/controller/path_tracker/pure_pursuit.py +7 -1
- python_motion_planning/controller/path_tracker/rpp.py +111 -0
- python_motion_planning/path_planner/__init__.py +2 -1
- python_motion_planning/path_planner/base_path_planner.py +45 -11
- python_motion_planning/path_planner/graph_search/__init__.py +4 -1
- python_motion_planning/path_planner/graph_search/a_star.py +12 -14
- python_motion_planning/path_planner/graph_search/dijkstra.py +15 -15
- python_motion_planning/path_planner/graph_search/gbfs.py +100 -0
- python_motion_planning/path_planner/graph_search/jps.py +199 -0
- python_motion_planning/path_planner/graph_search/lazy_theta_star.py +113 -0
- python_motion_planning/path_planner/graph_search/theta_star.py +17 -19
- python_motion_planning/path_planner/hybrid_search/__init__.py +1 -0
- python_motion_planning/path_planner/hybrid_search/voronoi_planner.py +204 -0
- python_motion_planning/path_planner/sample_search/__init__.py +2 -1
- python_motion_planning/path_planner/sample_search/rrt.py +70 -28
- python_motion_planning/path_planner/sample_search/rrt_connect.py +237 -0
- python_motion_planning/path_planner/sample_search/rrt_star.py +201 -151
- {python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/METADATA +54 -19
- {python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/RECORD +36 -27
- python_motion_planning/common/env/robot/tmp.py +0 -404
- {python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/WHEEL +0 -0
- {python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/licenses/LICENSE +0 -0
- {python_motion_planning-2.0.dev1.dist-info → python_motion_planning-2.0.dev2.dist-info}/top_level.txt +0 -0
|
@@ -1,9 +1,9 @@
|
|
|
1
1
|
"""
|
|
2
2
|
@file: dijkstra.py
|
|
3
3
|
@author: Wu Maojia
|
|
4
|
-
@update: 2025.10.
|
|
4
|
+
@update: 2025.10.6
|
|
5
5
|
"""
|
|
6
|
-
from typing import Union
|
|
6
|
+
from typing import Union, List, Tuple, Dict, Any
|
|
7
7
|
import heapq
|
|
8
8
|
|
|
9
9
|
from python_motion_planning.common import BaseMap, Grid, Node, TYPES
|
|
@@ -15,6 +15,7 @@ class Dijkstra(BasePathPlanner):
|
|
|
15
15
|
|
|
16
16
|
Args:
|
|
17
17
|
*args: see the parent class.
|
|
18
|
+
diagonal: whether to allow diagonal expansions.
|
|
18
19
|
*kwargs: see the parent class.
|
|
19
20
|
|
|
20
21
|
References:
|
|
@@ -23,26 +24,29 @@ class Dijkstra(BasePathPlanner):
|
|
|
23
24
|
Examples:
|
|
24
25
|
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
|
|
25
26
|
>>> planner = Dijkstra(map_=map_, start=(5, 5), goal=(10, 10))
|
|
26
|
-
>>> planner.plan()
|
|
27
|
-
([
|
|
28
|
-
|
|
27
|
+
>>> path, path_info = planner.plan()
|
|
28
|
+
>>> print(path_info['success'])
|
|
29
|
+
True
|
|
30
|
+
|
|
29
31
|
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
|
|
30
|
-
>>> planner.plan()
|
|
31
|
-
([
|
|
32
|
+
>>> path, path_info = planner.plan()
|
|
33
|
+
>>> print(path_info['success'])
|
|
34
|
+
True
|
|
32
35
|
"""
|
|
33
|
-
def __init__(self, *args, **kwargs) -> None:
|
|
36
|
+
def __init__(self, *args, diagonal: bool = True, **kwargs) -> None:
|
|
34
37
|
super().__init__(*args, **kwargs)
|
|
38
|
+
self.diagonal = diagonal
|
|
35
39
|
|
|
36
40
|
def __str__(self) -> str:
|
|
37
41
|
return "Dijkstra"
|
|
38
42
|
|
|
39
|
-
def plan(self) -> Union[
|
|
43
|
+
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
|
|
40
44
|
"""
|
|
41
45
|
Interface for planning.
|
|
42
46
|
|
|
43
47
|
Returns:
|
|
44
48
|
path: A list containing the path waypoints
|
|
45
|
-
path_info: A dictionary containing the path information
|
|
49
|
+
path_info: A dictionary containing the path information
|
|
46
50
|
"""
|
|
47
51
|
# OPEN list (priority queue) and CLOSED list (hash table)
|
|
48
52
|
OPEN = []
|
|
@@ -54,10 +58,6 @@ class Dijkstra(BasePathPlanner):
|
|
|
54
58
|
while OPEN:
|
|
55
59
|
node = heapq.heappop(OPEN)
|
|
56
60
|
|
|
57
|
-
# obstacle found
|
|
58
|
-
if not self.map_.is_expandable(node.current, node.parent):
|
|
59
|
-
continue
|
|
60
|
-
|
|
61
61
|
# exists in CLOSED list
|
|
62
62
|
if node.current in CLOSED:
|
|
63
63
|
continue
|
|
@@ -75,7 +75,7 @@ class Dijkstra(BasePathPlanner):
|
|
|
75
75
|
"expand": CLOSED
|
|
76
76
|
}
|
|
77
77
|
|
|
78
|
-
for node_n in self.map_.get_neighbors(node):
|
|
78
|
+
for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal):
|
|
79
79
|
# exists in CLOSED list
|
|
80
80
|
if node_n.current in CLOSED:
|
|
81
81
|
continue
|
|
@@ -0,0 +1,100 @@
|
|
|
1
|
+
"""
|
|
2
|
+
@file: gbfs.py
|
|
3
|
+
@author: Wu Maojia
|
|
4
|
+
@update: 2025.10.6
|
|
5
|
+
"""
|
|
6
|
+
from typing import Union, List, Tuple, Dict, Any
|
|
7
|
+
import heapq
|
|
8
|
+
|
|
9
|
+
from python_motion_planning.common import BaseMap, Grid, Node, TYPES
|
|
10
|
+
from python_motion_planning.path_planner.graph_search.dijkstra import Dijkstra
|
|
11
|
+
|
|
12
|
+
class GBFS(Dijkstra):
|
|
13
|
+
"""
|
|
14
|
+
Class for Greedy Best-First Search (GBFS) path planner.
|
|
15
|
+
|
|
16
|
+
Args:
|
|
17
|
+
*args: see the parent class.
|
|
18
|
+
*kwargs: see the parent class.
|
|
19
|
+
|
|
20
|
+
References:
|
|
21
|
+
[1] Heuristics: Intelligent Search Strategies for Computer Problem Solving.
|
|
22
|
+
|
|
23
|
+
Examples:
|
|
24
|
+
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
|
|
25
|
+
>>> planner = GBFS(map_=map_, start=(5, 5), goal=(10, 10))
|
|
26
|
+
>>> path, path_info = planner.plan()
|
|
27
|
+
>>> print(path_info['success'])
|
|
28
|
+
True
|
|
29
|
+
|
|
30
|
+
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
|
|
31
|
+
>>> path, path_info = planner.plan()
|
|
32
|
+
>>> print(path_info['success'])
|
|
33
|
+
True
|
|
34
|
+
"""
|
|
35
|
+
def __init__(self, *args, **kwargs) -> None:
|
|
36
|
+
super().__init__(*args, **kwargs)
|
|
37
|
+
|
|
38
|
+
def __str__(self) -> str:
|
|
39
|
+
return "GBFS"
|
|
40
|
+
|
|
41
|
+
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
|
|
42
|
+
"""
|
|
43
|
+
Interface for planning.
|
|
44
|
+
|
|
45
|
+
Returns:
|
|
46
|
+
path: A list containing the path waypoints
|
|
47
|
+
path_info: A dictionary containing the path information
|
|
48
|
+
"""
|
|
49
|
+
# OPEN list (priority queue) and CLOSED list (hash table)
|
|
50
|
+
OPEN = []
|
|
51
|
+
# For GBFS, we only use h-value (ignore g-value)
|
|
52
|
+
start_node = Node(self.start, None, 0, self.get_heuristic(self.start))
|
|
53
|
+
heapq.heappush(OPEN, start_node)
|
|
54
|
+
CLOSED = dict()
|
|
55
|
+
|
|
56
|
+
while OPEN:
|
|
57
|
+
node = heapq.heappop(OPEN)
|
|
58
|
+
|
|
59
|
+
# obstacle found
|
|
60
|
+
if not self.map_.is_expandable(node.current, node.parent):
|
|
61
|
+
continue
|
|
62
|
+
|
|
63
|
+
# exists in CLOSED list
|
|
64
|
+
if node.current in CLOSED:
|
|
65
|
+
continue
|
|
66
|
+
|
|
67
|
+
# goal found
|
|
68
|
+
if node.current == self.goal:
|
|
69
|
+
CLOSED[node.current] = node
|
|
70
|
+
path, length, cost = self.extract_path(CLOSED)
|
|
71
|
+
return path, {
|
|
72
|
+
"success": True,
|
|
73
|
+
"start": self.start,
|
|
74
|
+
"goal": self.goal,
|
|
75
|
+
"length": length,
|
|
76
|
+
"cost": cost,
|
|
77
|
+
"expand": CLOSED
|
|
78
|
+
}
|
|
79
|
+
|
|
80
|
+
for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal):
|
|
81
|
+
# exists in CLOSED list
|
|
82
|
+
if node_n.current in CLOSED:
|
|
83
|
+
continue
|
|
84
|
+
|
|
85
|
+
# For GBFS, we only set h-value
|
|
86
|
+
node_n.g = node.g + self.get_cost(node.current, node_n.current)
|
|
87
|
+
node_n.h = self.get_heuristic(node_n.current)
|
|
88
|
+
|
|
89
|
+
# goal found
|
|
90
|
+
if node_n.current == self.goal:
|
|
91
|
+
heapq.heappush(OPEN, node_n)
|
|
92
|
+
break
|
|
93
|
+
|
|
94
|
+
# update OPEN list with node sorted by h-value
|
|
95
|
+
heapq.heappush(OPEN, node_n)
|
|
96
|
+
|
|
97
|
+
CLOSED[node.current] = node
|
|
98
|
+
|
|
99
|
+
self.failed_info[1]["expand"] = CLOSED
|
|
100
|
+
return self.failed_info
|
|
@@ -0,0 +1,199 @@
|
|
|
1
|
+
"""
|
|
2
|
+
@file: jps.py
|
|
3
|
+
@author: Wu Maojia, Yang Haodong
|
|
4
|
+
@update: 2025.10.16
|
|
5
|
+
"""
|
|
6
|
+
from typing import Union, List, Tuple, Dict, Any
|
|
7
|
+
import heapq
|
|
8
|
+
from itertools import product
|
|
9
|
+
|
|
10
|
+
import numpy as np
|
|
11
|
+
|
|
12
|
+
from python_motion_planning.common import BaseMap, Grid, Node, TYPES
|
|
13
|
+
from python_motion_planning.path_planner.graph_search.a_star import AStar
|
|
14
|
+
|
|
15
|
+
class JPS(AStar):
|
|
16
|
+
"""
|
|
17
|
+
Class for Jump Point Search (JPS) path planner.
|
|
18
|
+
|
|
19
|
+
JPS is an optimization of the A* algorithm that identifies jump points to reduce the number of nodes
|
|
20
|
+
that need to be explored, significantly improving pathfinding efficiency on grid maps.
|
|
21
|
+
|
|
22
|
+
This implementation does not support non-diagonal option.
|
|
23
|
+
|
|
24
|
+
Args:
|
|
25
|
+
*args: see the parent class.
|
|
26
|
+
**kwargs: see the parent class.
|
|
27
|
+
|
|
28
|
+
References:
|
|
29
|
+
[1] Online Graph Pruning for Pathfinding On Grid Maps
|
|
30
|
+
|
|
31
|
+
Examples:
|
|
32
|
+
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
|
|
33
|
+
>>> planner = JPS(map_=map_, start=(5, 5), goal=(10, 10))
|
|
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
|
|
42
|
+
"""
|
|
43
|
+
def __init__(self, *args, **kwargs) -> None:
|
|
44
|
+
super().__init__(*args, **kwargs)
|
|
45
|
+
# Precompute all possible movement directions for N-dimensional grid
|
|
46
|
+
self.directions = self.map_._diagonal_offsets if self.diagonal else self.map_._orthogonal_offsets
|
|
47
|
+
|
|
48
|
+
def __str__(self) -> str:
|
|
49
|
+
return "JPS"
|
|
50
|
+
|
|
51
|
+
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
|
|
52
|
+
"""
|
|
53
|
+
Interface for planning using Jump Point Search.
|
|
54
|
+
|
|
55
|
+
Returns:
|
|
56
|
+
path: A list containing the path waypoints
|
|
57
|
+
path_info: A dictionary containing the path information
|
|
58
|
+
"""
|
|
59
|
+
# OPEN list (priority queue) and CLOSED list (hash table)
|
|
60
|
+
OPEN = []
|
|
61
|
+
start_node = Node(self.start, None, 0, self.get_heuristic(self.start))
|
|
62
|
+
heapq.heappush(OPEN, start_node)
|
|
63
|
+
CLOSED = dict()
|
|
64
|
+
|
|
65
|
+
while OPEN:
|
|
66
|
+
node = heapq.heappop(OPEN)
|
|
67
|
+
|
|
68
|
+
# Skip if node is already processed
|
|
69
|
+
if node.current in CLOSED:
|
|
70
|
+
continue
|
|
71
|
+
|
|
72
|
+
# Goal found
|
|
73
|
+
if node.current == self.goal:
|
|
74
|
+
CLOSED[node.current] = node
|
|
75
|
+
path, length, cost = self.extract_path(CLOSED)
|
|
76
|
+
return path, {
|
|
77
|
+
"success": True,
|
|
78
|
+
"start": self.start,
|
|
79
|
+
"goal": self.goal,
|
|
80
|
+
"length": length,
|
|
81
|
+
"cost": cost,
|
|
82
|
+
"expand": CLOSED
|
|
83
|
+
}
|
|
84
|
+
|
|
85
|
+
# Find all jump points from current node
|
|
86
|
+
jump_points = []
|
|
87
|
+
for direction in self.directions:
|
|
88
|
+
jp = self.jump(node, direction)
|
|
89
|
+
if jp and jp.current not in CLOSED:
|
|
90
|
+
jp.parent = node.current
|
|
91
|
+
jp.g = node.g + self.get_cost(node.current, jp.current)
|
|
92
|
+
jp.h = self.get_heuristic(jp.current)
|
|
93
|
+
jump_points.append(jp)
|
|
94
|
+
|
|
95
|
+
# Add jump points to OPEN list
|
|
96
|
+
for jp in jump_points:
|
|
97
|
+
heapq.heappush(OPEN, jp)
|
|
98
|
+
|
|
99
|
+
# Check if we found the goal
|
|
100
|
+
if jp.current == self.goal:
|
|
101
|
+
# Push goal node and break
|
|
102
|
+
heapq.heappush(OPEN, jp)
|
|
103
|
+
break
|
|
104
|
+
|
|
105
|
+
# Add current node to CLOSED list
|
|
106
|
+
CLOSED[node.current] = node
|
|
107
|
+
|
|
108
|
+
# If no path found
|
|
109
|
+
self.failed_info[1]["expand"] = CLOSED
|
|
110
|
+
return self.failed_info
|
|
111
|
+
|
|
112
|
+
def jump(self, node: Node, direction: Node) -> Union[Node, None]:
|
|
113
|
+
"""
|
|
114
|
+
Recursively search for jump points in a given direction.
|
|
115
|
+
|
|
116
|
+
Args:
|
|
117
|
+
node: Current node to start the jump from
|
|
118
|
+
direction: Direction of the jump (as a Node with integer coordinates)
|
|
119
|
+
|
|
120
|
+
Returns:
|
|
121
|
+
jump_point: The found jump point or None if no jump point exists
|
|
122
|
+
"""
|
|
123
|
+
# Calculate next node in the given direction
|
|
124
|
+
next_coords = tuple(n + d for n, d in zip(node.current, direction.current))
|
|
125
|
+
next_node = Node(next_coords, node.current, 0, 0)
|
|
126
|
+
|
|
127
|
+
# Check if next node is valid (within bounds and not an obstacle)
|
|
128
|
+
if not self.map_.is_expandable(next_node.current, node.current):
|
|
129
|
+
return None
|
|
130
|
+
|
|
131
|
+
# Check if we've reached the goal
|
|
132
|
+
if next_node.current == self.goal:
|
|
133
|
+
return next_node
|
|
134
|
+
|
|
135
|
+
# Check for forced neighbors
|
|
136
|
+
if self._has_forced_neighbors(next_node, direction):
|
|
137
|
+
return next_node
|
|
138
|
+
|
|
139
|
+
# For diagonal directions, check if we can jump in orthogonal directions
|
|
140
|
+
if all(d != 0 for d in direction.current):
|
|
141
|
+
# Check each orthogonal component of the diagonal direction
|
|
142
|
+
for i in range(self.dim):
|
|
143
|
+
if direction.current[i] != 0:
|
|
144
|
+
# Create orthogonal direction vector
|
|
145
|
+
ortho_dir = [0] * self.dim
|
|
146
|
+
ortho_dir[i] = direction.current[i]
|
|
147
|
+
ortho_dir = Node(tuple(ortho_dir))
|
|
148
|
+
|
|
149
|
+
# If there's a jump point in this orthogonal direction,
|
|
150
|
+
# current node is a jump point
|
|
151
|
+
if self.jump(next_node, ortho_dir):
|
|
152
|
+
return next_node
|
|
153
|
+
|
|
154
|
+
# Continue jumping in the same direction
|
|
155
|
+
return self.jump(next_node, direction)
|
|
156
|
+
|
|
157
|
+
def _has_forced_neighbors(self, node: Node, direction: Node) -> bool:
|
|
158
|
+
"""
|
|
159
|
+
Check if the current node has any forced neighbors that would make it a jump point.
|
|
160
|
+
|
|
161
|
+
Args:
|
|
162
|
+
node: Current node to check
|
|
163
|
+
direction: Direction of travel to reach this node
|
|
164
|
+
|
|
165
|
+
Returns:
|
|
166
|
+
bool: True if there are forced neighbors, False otherwise
|
|
167
|
+
"""
|
|
168
|
+
node_coords = node.current
|
|
169
|
+
dir_coords = direction.current
|
|
170
|
+
|
|
171
|
+
# Check all possible directions perpendicular to the current direction
|
|
172
|
+
for i in range(self.dim):
|
|
173
|
+
if dir_coords[i] == 0:
|
|
174
|
+
continue # Skip dimensions not part of current direction
|
|
175
|
+
|
|
176
|
+
# Check both positive and negative directions in perpendicular dimensions
|
|
177
|
+
for j in range(self.dim):
|
|
178
|
+
if i == j or dir_coords[j] != 0:
|
|
179
|
+
continue # Skip same dimension or already part of direction
|
|
180
|
+
|
|
181
|
+
# Check both directions in the perpendicular dimension
|
|
182
|
+
for sign in [-1, 1]:
|
|
183
|
+
# Calculate neighbor coordinates
|
|
184
|
+
neighbor = list(node_coords)
|
|
185
|
+
neighbor[j] += sign
|
|
186
|
+
neighbor = tuple(neighbor)
|
|
187
|
+
|
|
188
|
+
# Calculate opposite neighbor (obstacle check)
|
|
189
|
+
opposite = list(neighbor)
|
|
190
|
+
opposite[i] -= dir_coords[i]
|
|
191
|
+
opposite = tuple(opposite)
|
|
192
|
+
|
|
193
|
+
# If neighbor is valid and opposite is an obstacle,
|
|
194
|
+
# we have a forced neighbor
|
|
195
|
+
if (self.map_.is_expandable(neighbor, node.current) and
|
|
196
|
+
not self.map_.is_expandable(opposite, node.current)):
|
|
197
|
+
return True
|
|
198
|
+
|
|
199
|
+
return False
|
|
@@ -0,0 +1,113 @@
|
|
|
1
|
+
"""
|
|
2
|
+
@file: lazy_theta_star.py
|
|
3
|
+
@author: Wu Maojia, Yang Haodong
|
|
4
|
+
@update: 2025.10.6
|
|
5
|
+
"""
|
|
6
|
+
from typing import Union, List, Tuple, Dict, Any
|
|
7
|
+
import heapq
|
|
8
|
+
|
|
9
|
+
from python_motion_planning.common import BaseMap, Grid, Node, TYPES
|
|
10
|
+
from python_motion_planning.path_planner.graph_search.theta_star import ThetaStar
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
class LazyThetaStar(ThetaStar):
|
|
14
|
+
"""
|
|
15
|
+
Class for Lazy-Theta* path planner.
|
|
16
|
+
|
|
17
|
+
Args:
|
|
18
|
+
*args: see the parent class.
|
|
19
|
+
**kwargs: see the parent class.
|
|
20
|
+
|
|
21
|
+
References:
|
|
22
|
+
[1] Lazy Theta*: Any-Angle Path Planning and Path Length Analysis in 3D
|
|
23
|
+
|
|
24
|
+
Examples:
|
|
25
|
+
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
|
|
26
|
+
>>> planner = LazyThetaStar(map_=map_, start=(5, 5), goal=(10, 10))
|
|
27
|
+
>>> path, path_info = planner.plan()
|
|
28
|
+
>>> print(path_info['success'])
|
|
29
|
+
True
|
|
30
|
+
|
|
31
|
+
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
|
|
32
|
+
>>> path, path_info = planner.plan()
|
|
33
|
+
>>> print(path_info['success'])
|
|
34
|
+
True
|
|
35
|
+
"""
|
|
36
|
+
def __init__(self, *args, **kwargs) -> None:
|
|
37
|
+
super().__init__(*args, **kwargs)
|
|
38
|
+
|
|
39
|
+
def __str__(self) -> str:
|
|
40
|
+
return "Lazy-Theta*"
|
|
41
|
+
|
|
42
|
+
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
|
|
43
|
+
"""
|
|
44
|
+
Interface for planning.
|
|
45
|
+
|
|
46
|
+
Returns:
|
|
47
|
+
path: A list containing the path waypoints
|
|
48
|
+
path_info: A dictionary containing the path information
|
|
49
|
+
"""
|
|
50
|
+
# OPEN list (priority queue) and CLOSED list (hash table)
|
|
51
|
+
OPEN = []
|
|
52
|
+
start_node = Node(self.start, None, 0, self.get_heuristic(self.start))
|
|
53
|
+
heapq.heappush(OPEN, start_node)
|
|
54
|
+
CLOSED = dict()
|
|
55
|
+
|
|
56
|
+
while OPEN:
|
|
57
|
+
node = heapq.heappop(OPEN)
|
|
58
|
+
|
|
59
|
+
# ser vertex: path 1
|
|
60
|
+
node_p = CLOSED.get(node.parent)
|
|
61
|
+
if node_p:
|
|
62
|
+
if self.map_.in_collision(node_p.current, node.current):
|
|
63
|
+
node.g = float("inf")
|
|
64
|
+
for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal):
|
|
65
|
+
if node_n.current in CLOSED:
|
|
66
|
+
node_n = CLOSED.get(node_n.current)
|
|
67
|
+
if node.g > node_n.g + self.get_cost(node_n.current, node.current):
|
|
68
|
+
node.g = node_n.g + self.get_cost(node_n.current, node.current)
|
|
69
|
+
node.parent = node_n.current
|
|
70
|
+
|
|
71
|
+
# exists in CLOSED list
|
|
72
|
+
if node.current in CLOSED:
|
|
73
|
+
continue
|
|
74
|
+
|
|
75
|
+
# goal found
|
|
76
|
+
if node.current == self.goal:
|
|
77
|
+
CLOSED[node.current] = node
|
|
78
|
+
path, length, cost = self.extract_path(CLOSED)
|
|
79
|
+
return path, {
|
|
80
|
+
"success": True,
|
|
81
|
+
"start": self.start,
|
|
82
|
+
"goal": self.goal,
|
|
83
|
+
"length": length,
|
|
84
|
+
"cost": cost,
|
|
85
|
+
"expand": CLOSED
|
|
86
|
+
}
|
|
87
|
+
|
|
88
|
+
for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal):
|
|
89
|
+
# exists in CLOSED list
|
|
90
|
+
if node_n.current in CLOSED:
|
|
91
|
+
continue
|
|
92
|
+
|
|
93
|
+
# path1: normal A*
|
|
94
|
+
node_n.g = node.g + self.get_cost(node.current, node_n.current)
|
|
95
|
+
node_n.h = self.get_heuristic(node_n.current)
|
|
96
|
+
|
|
97
|
+
# path 2: Theta* line of sight update
|
|
98
|
+
node_p = CLOSED.get(node.parent)
|
|
99
|
+
if node_p:
|
|
100
|
+
self.updateVertex(node_p, node_n)
|
|
101
|
+
|
|
102
|
+
# goal found
|
|
103
|
+
if node_n.current == self.goal:
|
|
104
|
+
heapq.heappush(OPEN, node_n)
|
|
105
|
+
break
|
|
106
|
+
|
|
107
|
+
# update OPEN list
|
|
108
|
+
heapq.heappush(OPEN, node_n)
|
|
109
|
+
|
|
110
|
+
CLOSED[node.current] = node
|
|
111
|
+
|
|
112
|
+
self.failed_info[1]["expand"] = CLOSED
|
|
113
|
+
return self.failed_info
|
|
@@ -1,9 +1,9 @@
|
|
|
1
1
|
"""
|
|
2
2
|
@file: theta_star.py
|
|
3
3
|
@author: Wu Maojia, Yang Haodong
|
|
4
|
-
@update: 2025.10.
|
|
4
|
+
@update: 2025.10.6
|
|
5
5
|
"""
|
|
6
|
-
from typing import Union
|
|
6
|
+
from typing import Union, List, Tuple, Dict, Any
|
|
7
7
|
import heapq
|
|
8
8
|
|
|
9
9
|
from python_motion_planning.common import BaseMap, Grid, Node, TYPES
|
|
@@ -25,12 +25,14 @@ class ThetaStar(AStar):
|
|
|
25
25
|
Examples:
|
|
26
26
|
>>> map_ = Grid(bounds=[[0, 15], [0, 15]])
|
|
27
27
|
>>> planner = ThetaStar(map_=map_, start=(5, 5), goal=(10, 10))
|
|
28
|
-
>>> planner.plan()
|
|
29
|
-
([
|
|
30
|
-
|
|
28
|
+
>>> path, path_info = planner.plan()
|
|
29
|
+
>>> print(path_info['success'])
|
|
30
|
+
True
|
|
31
|
+
|
|
31
32
|
>>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
|
|
32
|
-
>>> planner.plan()
|
|
33
|
-
([
|
|
33
|
+
>>> path, path_info = planner.plan()
|
|
34
|
+
>>> print(path_info['success'])
|
|
35
|
+
True
|
|
34
36
|
"""
|
|
35
37
|
def __init__(self, *args, **kwargs) -> None:
|
|
36
38
|
super().__init__(*args, **kwargs)
|
|
@@ -38,13 +40,13 @@ class ThetaStar(AStar):
|
|
|
38
40
|
def __str__(self) -> str:
|
|
39
41
|
return "Theta*"
|
|
40
42
|
|
|
41
|
-
def plan(self) ->
|
|
43
|
+
def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
|
|
42
44
|
"""
|
|
43
45
|
Interface for planning.
|
|
44
46
|
|
|
45
47
|
Returns:
|
|
46
48
|
path: A list containing the path waypoints
|
|
47
|
-
path_info: A dictionary containing the path information
|
|
49
|
+
path_info: A dictionary containing the path information
|
|
48
50
|
"""
|
|
49
51
|
# OPEN list (priority queue) and CLOSED list (hash table)
|
|
50
52
|
OPEN = []
|
|
@@ -55,10 +57,6 @@ class ThetaStar(AStar):
|
|
|
55
57
|
while OPEN:
|
|
56
58
|
node = heapq.heappop(OPEN)
|
|
57
59
|
|
|
58
|
-
# obstacle found
|
|
59
|
-
if not self.map_.is_expandable(node.current, node.parent):
|
|
60
|
-
continue
|
|
61
|
-
|
|
62
60
|
# exists in CLOSED list
|
|
63
61
|
if node.current in CLOSED:
|
|
64
62
|
continue
|
|
@@ -76,7 +74,7 @@ class ThetaStar(AStar):
|
|
|
76
74
|
"expand": CLOSED
|
|
77
75
|
}
|
|
78
76
|
|
|
79
|
-
for node_n in self.map_.get_neighbors(node):
|
|
77
|
+
for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal):
|
|
80
78
|
# exists in CLOSED list
|
|
81
79
|
if node_n.current in CLOSED:
|
|
82
80
|
continue
|
|
@@ -89,7 +87,8 @@ class ThetaStar(AStar):
|
|
|
89
87
|
# path 2: Theta* line of sight update
|
|
90
88
|
node_p = CLOSED.get(node.parent)
|
|
91
89
|
if node_p:
|
|
92
|
-
self.
|
|
90
|
+
if not self.map_.in_collision(node_p.current, node_n.current):
|
|
91
|
+
self.updateVertex(node_p, node_n)
|
|
93
92
|
|
|
94
93
|
# goal found
|
|
95
94
|
if node_n.current == self.goal:
|
|
@@ -112,7 +111,6 @@ class ThetaStar(AStar):
|
|
|
112
111
|
node_p (Node): parent node
|
|
113
112
|
node_n (Node): next node
|
|
114
113
|
"""
|
|
115
|
-
if
|
|
116
|
-
|
|
117
|
-
|
|
118
|
-
node_n.parent = node_p.current
|
|
114
|
+
if node_p.g + self.get_cost(node_p.current, node_n.current) <= node_n.g:
|
|
115
|
+
node_n.g = node_p.g + self.get_cost(node_p.current, node_n.current)
|
|
116
|
+
node_n.parent = node_p.current
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
from .voronoi_planner import *
|