def generate_path(self, task):
self.path_list = []
for path_index in range(self.path_num):
ref = ReferencePath(task)
ref.set_path(path_index)
self.path_list.append(ref)
return self.path_list
def construct_ref_path(self, task, state, v_light):
x, y, phi = state[3], state[4], state[5] / 180 * pi
state = [x, y, phi]
# The total trajectory consists of three different parts.
def traj_start_straight(path_index, state):
if len(self.feature_points[path_index]) == len(self.feature_points_all[path_index]) and state[1] < -CROSSROAD_SIZE / 2:
start_line_y = np.linspace(state[1], -CROSSROAD_SIZE / 2, int(abs(state[1]-(-CROSSROAD_SIZE / 2))) * meter_pointnum_ratio, dtype=np.float32)[:-1]
start_line_x = np.linspace(state[0], LANE_WIDTH / 2, int(abs(state[1]-(-CROSSROAD_SIZE / 2))) * meter_pointnum_ratio, dtype=np.float32)[:-1]
else:
start_line_x, start_line_y = [], []
return start_line_x, start_line_y
def traj_cross(path_index, state):
if len(self.feature_points[path_index]) == 4: # the vehicle has not passed into the crossing
feature_point1, feature_point2 = self.feature_points[path_index][0], self.feature_points[path_index][1]
self.L0, self.L3 = 15. * np.ones([self.path_num]), 15. * np.ones([self.path_num])
cross_x, cross_y = self.trajectory_planning(feature_point1, feature_point2, feature_point2, self.L0[path_index], self.L3[path_index])
elif len(self.feature_points[path_index]) == 1:
feature_point1 = self.feature_points[path_index][0]
cross_x, cross_y = self.trajectory_planning(state, feature_point1, feature_point1, self.L0[path_index],
self.L3[path_index])
else:
feature_point1, feature_point2 = self.feature_points[path_index][0], self.feature_points[path_index][1]
cross_x, cross_y = self.trajectory_planning(state, feature_point1, feature_point2, self.L0[path_index],
self.L3[path_index])
return cross_x, cross_y
def traj_end_straight(path_index):
if len(self.feature_points[path_index]) >= 3: # the vehicle has not arrived
end_line_y = self.end_offsets[path_index] * np.ones(shape=(sl * meter_pointnum_ratio), dtype=np.float32)[1:]
end_line_x = np.linspace(-CROSSROAD_SIZE / 2, -sl - CROSSROAD_SIZE/2, sl * meter_pointnum_ratio, dtype=np.float32)[1:]
elif len(self.feature_points[path_index]) == 2: # the vehicle has passed the intersection
end_line_y = self.end_offsets[path_index] * np.ones(shape=((sl - CROSSROAD_SIZE//2) * meter_pointnum_ratio), dtype=np.float32)[1:]
end_line_x = np.linspace(-CROSSROAD_SIZE, -sl - CROSSROAD_SIZE / 2, (sl - CROSSROAD_SIZE//2) * meter_pointnum_ratio, dtype=np.float32)[1:]
else: # the vehicle has arrived
end_line_x, end_line_y = [], []
return end_line_x, end_line_y
self.path_list = []
for path_index in range(0, self.path_num):
start_line_x, start_line_y = traj_start_straight(path_index, state)
cross_x, cross_y = traj_cross(path_index, state)
end_line_x, end_line_y = traj_end_straight(path_index)
total_x, total_y = np.append(start_line_x, np.append(cross_x, end_line_x)), np.append(start_line_y, np.append(cross_y, end_line_y))
total_x = total_x.astype(np.float32)
total_y = total_y.astype(np.float32)
xs_1, ys_1 = total_x[:-1], total_y[:-1]
xs_2, ys_2 = total_x[1:], total_y[1:]
phis_1 = np.arctan2(ys_2 - ys_1, xs_2 - xs_1) * 180 / pi
planed_trj = xs_1, ys_1, phis_1
ref = ReferencePath(task)
ref.set_path(self.mode, path_index=path_index, path=planed_trj)
self.path_list.append(ref)
def generate_traj(self, task, state=None, v_light=0):
""""generate reference trajectory in real time"""
if self.mode == 'static_traj':
self.path_list = []
for path_index in range(self.path_num):
ref = ReferencePath(task)
ref.set_path(path_index)
self.path_list.append(ref)
else:
self._future_point_choice(state, task)
self.construct_ref_path(task, state, v_light)
return self.path_list, self._future_points_init(task)