-
Notifications
You must be signed in to change notification settings - Fork 5
/
collision_checker.py
205 lines (181 loc) · 10.3 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
#!/usr/bin/env python3
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.
# Author: Ryan De Iaco
# Additional Comments: Carlos Wang
# Date: October 29, 2018
import numpy as np
import scipy.spatial
from math import sin, cos, pi, sqrt
class CollisionChecker:
def __init__(self, circle_offsets, circle_radii, weight):
self._circle_offsets = circle_offsets
self._circle_radii = circle_radii
self._weight = weight
######################################################
######################################################
# MODULE 7: CHECKING FOR COLLISSIONS
# Read over the function comments to familiarize yourself with the
# arguments and necessary variables to return. Then follow the TODOs
# (top-down) and use the surrounding comments as a guide.
######################################################
######################################################
# 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))
# TODO: INSERT YOUR CODE BETWEEN THE DASHED LINES
# --------------------------------------------------------------
#compute collision circle
x_circle_check = [i * cos(path[2][j]) for i in self._circle_offsets]
y_circle_check = [i * sin(path[2][j]) for i in self._circle_offsets]
#translate circle with center the path points
circle_locations[:, 0] = path[0][j] + x_circle_check
circle_locations[:, 1] = path[1][j] + y_circle_check
# --------------------------------------------------------------
# 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
######################################################
######################################################
# MODULE 7: SELECTING THE BEST PATH INDEX
# Read over the function comments to familiarize yourself with the
# arguments and necessary variables to return. Then follow the TODOs
# (top-down) and use the surrounding comments as a guide.
######################################################
######################################################
# 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.
# TODO: INSERT YOUR CODE BETWEEN THE DASHED LINES
# --------------------------------------------------------------
path = paths[i]
path_size = len(path[0]) - 1 #all elements(path[0] or 1 or 2) should give the same size
score = np.sqrt(pow(goal_state[0]-path[0][path_size], 2) + pow(goal_state[1]-path[1][path_size], 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]:
# TODO: INSERT YOUR CODE BETWEEN THE DASHED LINES
# --------------------------------------------------
colliding_path = paths[j]
colliding_path_size = len(colliding_path[0]) - 1
proximity_to_colliding_path = np.sqrt(pow(colliding_path[0][colliding_path_size]-path[0][path_size], 2) + pow(colliding_path[1][colliding_path_size]-path[1][path_size], 2))
#we use the 1/proximity_to_colliding_path, so we reward if the proximity_to_colliding_path has a big value
score += 1/proximity_to_colliding_path
# --------------------------------------------------
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