-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathcollision_checker.py
313 lines (273 loc) · 14.8 KB
/
collision_checker.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
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
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
#!/usr/bin/env python3
import numpy as np
import scipy.spatial
from math import sin, cos, pi, sqrt
from behavioural_planner import pointOnSegment
# EditGroup2
# Costanti
HALF_LANE_WIDTH = 2
BRAKE_DISTANCE = 3
DIRECTION_SCORE = 0.95
PEDESTRIAN_FENCE_LEN = 6
# EndEditGroup2
class CollisionChecker:
def __init__(self, circle_offsets, circle_radii, weight):
self._circle_offsets = circle_offsets
self._circle_radii = circle_radii
self._weight = weight
# Takes in a set of paths and obstacles, and returns an array
# of bools that says whether or not each path is collision free.
def collision_check(self, paths, obstacles):
"""Returns a bool array on whether each path is collision free.
args:
paths: A list of paths in the global frame.
A path is a list of points of the following format:
[x_points, y_points, t_points]:
x_points: List of x values (m)
y_points: List of y values (m)
t_points: List of yaw values (rad)
Example of accessing the ith path, jth point's t value:
paths[i][2][j]
obstacles: A list of [x, y] points that represent points along the
border of obstacles, in the global frame.
Format: [[x0, y0],
[x1, y1],
...,
[xn, yn]]
, where n is the number of obstacle points and units are [m, m]
returns:
collision_check_array: A list of boolean values which classifies
whether the path is collision-free (true), or not (false). The
ith index in the collision_check_array list corresponds to the
ith path in the paths list.
"""
collision_check_array = np.zeros(len(paths), dtype=bool)
for i in range(len(paths)):
collision_free = True
path = paths[i]
# Iterate over the points in the path.
for j in range(len(path[0])):
# Compute the circle locations along this point in the path.
# These circle represent an approximate collision
# border for the vehicle, which will be used to check
# for any potential collisions along each path with obstacles.
# The circle offsets are given by self._circle_offsets.
# The circle offsets need to placed at each point along the path,
# with the offset rotated by the yaw of the vehicle.
# Each path is of the form [[x_values], [y_values],
# [theta_values]], where each of x_values, y_values, and
# theta_values are in sequential order.
# Thus, we need to compute:
# circle_x = point_x + circle_offset*cos(yaw)
# circle_y = point_y circle_offset*sin(yaw)
# for each point along the path.
# point_x is given by path[0][j], and point _y is given by
# path[1][j].
circle_locations = np.zeros((len(self._circle_offsets), 2))
circle_offset = np.array(self._circle_offsets)
circle_locations[:, 0] = path[0][j] + circle_offset * cos(path[2][j])
circle_locations[:, 1] = path[1][j] + circle_offset * sin(path[2][j])
# Assumes each obstacle is approximated by a collection of
# points of the form [x, y].
# Here, we will iterate through the obstacle points, and check
# if any of the obstacle points lies within any of our circles.
# If so, then the path will collide with an obstacle and
# the collision_free flag should be set to false for this flag
for k in range(len(obstacles)):
collision_dists = \
scipy.spatial.distance.cdist(obstacles[k],
circle_locations)
collision_dists = np.subtract(collision_dists,
self._circle_radii)
collision_free = collision_free and \
not np.any(collision_dists < 0)
if not collision_free:
break
if not collision_free:
break
collision_check_array[i] = collision_free
return collision_check_array
def pedestrian_collision_check(self, paths, pedestrians, best_path):
"""Returns a goal state for the vehicle to reach and a boolean that
representes the collision with a pedestrian
args:
paths: A list of paths in the global frame.
A path is a list of points of the following format:
[x_points, y_points, t_points]:
x_points: List of x values (m)
y_points: List of y values (m)
t_points: List of yaw values (rad)
Example of accessing the ith path, jth point's t value:
paths[i][2][j]
pedestrians: A list of [x, y] points that represent points along the
border of obstacles, in the global frame.
Format: [[x0, y0],
[x1, y1],
...,
[xn, yn]]
, where n is the number of obstacle points and units are [m, m]
best_path: A list of points of the following format:
[x_points, y_points, t_points]:
x_points: List of x values (m)
y_points: List of y values (m)
t_points: List of yaw values (rad)
returns:
goal_state: Goal state for the vehicle to reach (global frame)
format: [x_goal, y_goal, v_goal]
boolean:
True if there has been a collision with a pedestrian,
False otherwise
"""
pedestrians_in_collision = set()
for i in range(len(paths)):
path = paths[i]
# Iterate over the points in the path.
for j in range(len(path[0])):
# Compute the circle locations along this point in the path.
# These circle represent an approximate collision
# border for the vehicle, which will be used to check
# for any potential collisions along each path with obstacles.
# The circle offsets are given by self._circle_offsets.
# The circle offsets need to placed at each point along the path,
# with the offset rotated by the yaw of the vehicle.
# Each path is of the form [[x_values], [y_values],
# [theta_values]], where each of x_values, y_values, and
# theta_values are in sequential order.
# Thus, we need to compute:
# circle_x = point_x + circle_offset*cos(yaw)
# circle_y = point_y circle_offset*sin(yaw)
# for each point along the path.
# point_x is given by path[0][j], and point _y is given by
# path[1][j].
circle_locations = np.zeros((len(self._circle_offsets), 2))
circle_offset = np.array(self._circle_offsets)
circle_locations[:, 0] = path[0][j] + circle_offset * cos(path[2][j])
circle_locations[:, 1] = path[1][j] + circle_offset * sin(path[2][j])
# Assumes each obstacle is approximated by a collection of
# points of the form [x, y].
# Here, we will iterate through the obstacle points, and check
# if any of the obstacle points lies within any of our circles.
# If so, then the path will collide with an obstacle and
# the collision_free flag should be set to false for this flag
for k in range(len(pedestrians)):
collision_free = True
collision_dists = \
scipy.spatial.distance.cdist(pedestrians[k][1],
circle_locations)
collision_dists = np.subtract(collision_dists,
self._circle_radii)
collision_free = collision_free and \
not np.any(collision_dists < 0)
if not collision_free:
pedestrians_in_collision.add(k)
# Iteriamo su tutti i pedoni che sono in collisioni con i nostri
# path locali. Per ogni pedone creiamo una fence e controlliamo se
# tale fence interseca con il nostro best path. In tal caso possiamo
# dire che il pedone sta effettivamente attraversando la strada.
# Quindi costruiamo il goal state in modo da fermarci prima del pedone
# e non investirlo
for index in pedestrians_in_collision:
pedestrian = pedestrians[index][0]
pedestrian_yaw = pedestrian.transform.rotation.yaw * pi / 180
pedestrian_x = pedestrian.transform.location.x
pedestrian_y = pedestrian.transform.location.y
# x e y finali della fence
pedestrian_final_x = pedestrian_x + PEDESTRIAN_FENCE_LEN * cos(pedestrian_yaw)
pedestrian_final_y = pedestrian_y + PEDESTRIAN_FENCE_LEN * sin(pedestrian_yaw)
# x e y iniziali della fence
pedestrian_x = pedestrian.transform.location.x - PEDESTRIAN_FENCE_LEN / 2 * cos(pedestrian_yaw)
pedestrian_y = pedestrian.transform.location.y - PEDESTRIAN_FENCE_LEN / 2 * sin(pedestrian_yaw)
intersect_flag = False
for i in range(len(best_path[0])-1):
wp_1 = [best_path[0][i], best_path[1][i]]
wp_2 = [best_path[0][i+1], best_path[1][i+1]]
s_1 = [pedestrian_x, pedestrian_y]
s_2 = [pedestrian_final_x, pedestrian_final_y]
v1 = np.subtract(wp_2, wp_1)
v2 = np.subtract(s_1, wp_2)
sign_1 = np.sign(np.cross(v1, v2))
v2 = np.subtract(s_2, wp_2)
sign_2 = np.sign(np.cross(v1, v2))
v1 = np.subtract(s_2, s_1)
v2 = np.subtract(wp_1, s_2)
sign_3 = np.sign(np.cross(v1, v2))
v2 = np.subtract(wp_2, s_2)
sign_4 = np.sign(np.cross(v1, v2))
# Check if the line segments intersect.
if (sign_1 != sign_2) and (sign_3 != sign_4):
intersect_flag = True
# Check if the collinearity cases hold.
if (sign_1 == 0) and pointOnSegment(wp_1, s_1, wp_2):
intersect_flag = True
if (sign_2 == 0) and pointOnSegment(wp_1, s_2, wp_2):
intersect_flag = True
if (sign_3 == 0) and pointOnSegment(s_1, wp_1, s_2):
intersect_flag = True
if (sign_3 == 0) and pointOnSegment(s_1, wp_2, s_2):
intersect_flag = True
if intersect_flag:
for j in reversed(range(0, i+1)):
if ((((best_path[0][i] - best_path[0][j] )**2) + ((best_path[1][i] - best_path[1][j] )**2) )**0.5) >= BRAKE_DISTANCE:
return [best_path[0][j], best_path[1][j], 0], True
return [best_path[0][0], best_path[1][0], 0], True
return [], False
# Selects the best path in the path set, according to how closely
# it follows the lane centerline, and how far away it is from other
# paths that are in collision.
# Disqualifies paths that collide with obstacles from the selection
# process.
# collision_check_array contains True at index i if paths[i] is
# collision-free, otherwise it contains False.
def select_best_path_index(self, paths, collision_check_array, goal_state):
"""Returns the path index which is best suited for the vehicle to
traverse.
Selects a path index which is closest to the center line as well as far
away from collision paths.
args:
paths: A list of paths in the global frame.
A path is a list of points of the following format:
[x_points, y_points, t_points]:
x_points: List of x values (m)
y_points: List of y values (m)
t_points: List of yaw values (rad)
Example of accessing the ith path, jth point's t value:
paths[i][2][j]
collision_check_array: A list of boolean values which classifies
whether the path is collision-free (true), or not (false). The
ith index in the collision_check_array list corresponds to the
ith path in the paths list.
goal_state: Goal state for the vehicle to reach (centerline goal).
format: [x_goal, y_goal, v_goal], unit: [m, m, m/s]
useful variables:
self._weight: Weight that is multiplied to the best index score.
returns:
best_index: The path index which is best suited for the vehicle to
navigate with.
"""
best_index = None
best_score = float('Inf')
for i in range(len(paths)):
# Handle the case of collision-free paths.
if collision_check_array[i]:
# Compute the "distance from centerline" score.
# The centerline goal is given by goal_state.
# The exact choice of objective function is up to you.
# A lower score implies a more suitable path.
score = np.sqrt((paths[i][0][-1]-goal_state[0])**2+(paths[i][1][-1]-goal_state[1])**2)
# Compute the "proximity to other colliding paths" score and
# add it to the "distance from centerline" score.
# The exact choice of objective function is up to you.
for j in range(len(paths)):
if j == i:
continue
else:
if not collision_check_array[j]:
pass
# Handle the case of colliding paths.
else:
score = float('Inf')
# Set the best index to be the path index with the lowest score
if score < best_score:
best_score = score
best_index = i
return best_index