-
Notifications
You must be signed in to change notification settings - Fork 0
/
route_opt_utils.py
147 lines (118 loc) · 4.44 KB
/
route_opt_utils.py
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
import random
import math
from icecream import ic
class RiskZone:
def __init__(self,location:tuple[int],radius:(int)) -> None:
self.location = location
self.radius = radius
self.radiusLOS = radius
class CircleObstacle:
def __init__(self,location:tuple[int],radius:(int)) -> None:
self.location = location
self.radius = radius
self.radiusLOS = radius
class SquareObstacle:
def __init__(self) -> None:
pass
class TreeNode:
def __init__(self,location:tuple[int]) -> None:
self.location = location
self.parent = None
self.children = []
self.path_child = None
self.pathLOS_parent = None
self.pathLOS_child = None
self.memberofpath = False
self.node_cost = 0
self.path_cost = 0
self.nodeLOS_cost = 0
self.pathLOS_cost = 0
self.neighbourhood = []
class TreePath:
def __init__(self,location:tuple[int]) -> None:
self.location = location
self.nodes = [] # includes startnode
self.path_cost = 0
class TreeResults:
def __init__(self,
start_location:tuple[int],
goal_location:tuple[int]
) -> None:
self.start_location = start_location
self.goal_location = goal_location
self.randomtree = [] #todo hoe goed gebruiken?
self.goalpath = []
self.goalpathLOS = []
# draw random point within borders of map
def draw_random_sample(mapwidth:int,mapheight:int) -> int:
x = int(random.uniform(0,mapwidth))
y = int(random.uniform(0,mapheight))
randompoint = (x,y)
return randompoint
def clear_treepath(path:TreePath) -> None:
path.nodes = []
path.path_cost = 0
# determine distance between 2 nodes
def node_distance(node1:TreeNode,node2:TreeNode) -> float:
nodedistance = math.dist(node1.location,node2.location)
return nodedistance
# check if node is in free space or within circle obstacle
def is_freespace_circle(node:TreeNode,
obstacles:CircleObstacle,
margin:int
) -> bool:
# check for all obstacles
for circle in obstacles:
# collision when point is within circle radius + margin
if (math.dist((node.location),(circle.location))
<= (circle.radius + margin)):
return False
# no collision detected
return True
# check if connection crosses a circle obstacle
def cross_circle_obstacle(node1:TreeNode,
node2:TreeNode,
obstacles:CircleObstacle,
margin:int
) -> bool:
# init
line_length = math.dist(node1.location,node2.location)
circle:CircleObstacle
# check for all obstacles
for circle in obstacles:
# check if line is far enough away from circle to not cross
dist1 = math.dist(node1.location,circle.location)
dist2 = math.dist(node2.location,circle.location)
if (dist1 or dist2) > (line_length + circle.radius + margin):
continue
# divide connection in 100 points to check each
for i in range(0,101):
u = i/100
x = node1.location[0] * u + node2.location[0] * (1-u)
y = node1.location[1] * u + node2.location[1] * (1-u)
# collision when point is within circle radius + margin
if math.dist((x,y),(circle.location)) <= (circle.radius + margin):
return True
# no collision detected
return False
# measure distance of node to every node in tree, return nearest
def nearest_neighbour(node:TreeNode,
startnode:TreeNode,
randomtree:TreePath,
RRTstar:bool
) -> TreeNode:
# initialise
d_min = 100000
nearestnode = startnode
# find nearest node in random tree
for neighbournode in randomtree.nodes:
# nodes already member of a path are not used for neasest neighbour in RRT
if not RRTstar and neighbournode.memberofpath:
continue
# check if neighbour is nearest neighbour
distance = node_distance(neighbournode,node)
if distance < d_min:
d_min = distance
nearestnode = neighbournode
# return nearest neighbour node
return nearestnode