-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathdata_structure.py
More file actions
197 lines (184 loc) · 6.71 KB
/
Copy pathdata_structure.py
File metadata and controls
197 lines (184 loc) · 6.71 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
import numpy as np
import math
from heap import BinaryHeap as Heap
# from simple_edge import SimpleEdge as Edge
from edge import Edge
# from dubins_edge import DubinsEdge as Edge
class RRTNode(object):
def __init__(self,
position,
kd_in_tree=False,
kd_parent_exist=False,
kd_child_l_exist=False,
kd_child_r_exist=False,
heap_index=-1,
in_heap=False,
rrt_parent_used=False,
priority_queue_index=-1,
in_priority_queue=False,
in_os_queue=False,
is_move_goal=False,
):
self.kd_in_tree = kd_in_tree
self.kd_parent_exist = kd_parent_exist
self.kd_in_tree = kd_in_tree
self.kd_parent_exist = kd_parent_exist
self.kd_child_l_exist = kd_child_l_exist
self.kd_child_r_exist = kd_child_r_exist
self.heap_index = heap_index
self.in_heap = in_heap
self.rrt_parent_used = rrt_parent_used
self.rrt_neighbors_out = []
self.rrt_neighbors_in = []
self.priority_queue_index = priority_queue_index
self.in_priority_queue = in_priority_queue
self.successor_list = []
self.initial_neighbor_list_out = []
self.initial_neighbor_list_in = []
self.in_os_queue = in_os_queue
self.is_move_goal = is_move_goal
self.position = position
self.kd_split = None
self.kd_parent: RRTNode = None
self.kd_child_l: RRTNode = None
self.kd_child_r: RRTNode = None
self.rrt_parent_edge: Edge = None
self.rrt_tree_cost = None
self.rrt_lmc = None
self.rrt_h = None
self.temp_edge: Edge = None
self.successor_list_item_in_parent = None
self.has_certificate = False
self.certificate_value = None
self.certifying_node = None
self.out_neighbor_counter = 0
self.initial_out_neighbor_counter = 0
self.in_neighbor_counter = 0
self.initial_in_neighbor_counter = 0
class Obstacle(object):
def __init__(self,
kind,
position=None,
radius=None,
span=None,
polygon = None,
direction = None,
prism_span_min = None,
prism_span_max = None,
start_time=0.0,
life_span=math.inf,
obstacle_unused=False,
senseable_obstacle=False,
obstacle_unused_after_sense=True,
):
self.kind = kind
self.start_time = start_time
self.life_span = life_span
self.obstacle_unused = obstacle_unused
self.senseable_obstacle = senseable_obstacle
self.obstacle_unused_after_sense = obstacle_unused_after_sense
self.position = position
self.radius = radius
self.span = span
self.polygon = polygon
self.direction = direction
self.prism_span_min = prism_span_min
self.prism_span_max = prism_span_min
self.velocity = None
self.path = None
self.original_polygon = None
self.unknown_path = None
self.next_direction_change_time = None
self.next_direction_change_ind = None
self.last_direction_change_time = None
if radius is None and span is not None:
self.radius = np.linalg.norm(span)
if polygon is not None:
polygon_array = np.array(polygon)
max_x = np.max(polygon_array[:,0])
min_x = np.min(polygon_array[:,0])
max_y = np.max(polygon_array[:,1])
min_y = np.min(polygon_array[:,1])
self.position = np.array([max_x + min_x, max_y + min_y])/2.0
self.radius = np.sqrt(np.max(np.sum((polygon_array - self.position)**2, 1)))
self.span = np.array([-1.0, 1.0])
class CSpace(object):
def __init__(self,
d,
obs_delta,
lower_bounds,
upper_bounds,
start,
goal):
self.d = d
self.obstacles = []
self.obs_delta = obs_delta
self.lower_bounds = lower_bounds
self.upper_bounds = upper_bounds
self.width = upper_bounds - lower_bounds
self.start = start
self.goal = goal
self.space_has_time = False
self.space_has_theta = False
self.p_goal = None
self.rand_node = None
self.goal_node: RRTNode = None
self.root: RRTNode = None
self.move_goal: RRTNode = None
self.its_until_sample = None
self.its_sample_point = None
self.time_sample_point = None
self.wait_time = None
self.start_time_ns = None
self.elapsed_time = None
self.obstacles_to_remove: Obstacle = None
self.robot_radius = None
self.robot_velocity = None
self.dubins_min_velocity = None
self.dubins_max_velocity = None
self.sample_stack = None
self.hyper_volume = 0.0
self.delta = None
self.min_turning_radius = None
self.file_ctr = None
self.warmup_time = 0.0
self.in_warmup_time = False
class rrtXQueue(object):
def __init__(self):
self.Q: Heap = None
self.OS = None
self.S: CSpace = None
self.change_thresh = None
class RRTNodeNeighborIterator(object):
def __init__(self,
this_node,
list_flag=0,
list_item=None):
self.this_node = this_node
self.list_flag = 0
self.list_itemNode = list_item
self.counter = 0
class RobotData(object):
def __init__(self,
robot_pose,
next_move_target,
max_path_nodes):
self.robot_pose = robot_pose
self.next_robot_pose = robot_pose
self.next_move_target = next_move_target
self.distance_from_next_robot_pose_to_next_move_target = 0.0
self.moving = False
self.current_move_invalid = False
self.robot_move_path = [robot_pose]
self.num_robot_move_points = 1
self.robot_local_path = []
self.num_local_move_points = 0
self.max_path_nodes = max_path_nodes
self.robot_edge: Edge = None
self.robot_edge_used = False
self.dist_along_robot_edge = 0.0
self.time_along_robot_edge = 0.0
self.robot_edge_for_plotting_used =False
self.robot_edge_for_plotting: Edge = None
self.dist_along_robot_edge_for_plotting = 0.0
self.time_along_robot_edge_for_plotting = 0.0