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 __init__(self, x_init, num_future_data, ref_index, task, exp_v, tau, veh_mode_list, per_veh_info_dim=4):
self.task = task
self.exp_v = exp_v
self.tau = tau
self.per_veh_info_dim = per_veh_info_dim
self.vd = VehicleDynamics()
self.veh_mode_list = veh_mode_list
self.vehs = x_init[6+3*(1+num_future_data):]
self.x_init = x_init
path = ReferencePath(task)
self.ref_index = ref_index
path = path.path_list[self.ref_index]
x, y, phi = [ite[1200:-1200] for ite in path]
if self.task == 'left':
self.start, self.end = x[0], y[-1]
fit_x = np.arctan2(y - (-CROSSROAD_SIZE / 2), x - (-CROSSROAD_SIZE / 2))
fit_y1 = np.sqrt(np.square(x - (-CROSSROAD_SIZE / 2)) + np.square(y - (-CROSSROAD_SIZE / 2)))
fit_y2 = phi
elif self.task == 'straight':
self.start, self.end = x[0], x[-1]
fit_x = y
fit_y1 = x
fit_y2 = phi
else:
self.start, self.end = x[0], y[-1]
fit_x = np.arctan2(y - (-CROSSROAD_SIZE / 2), x - (CROSSROAD_SIZE / 2))
fit_y1 = np.sqrt(np.square(x - (CROSSROAD_SIZE / 2)) + np.square(y - (-CROSSROAD_SIZE / 2)))
fit_y2 = phi
self.fit_y1_para = list(np.polyfit(fit_x, fit_y1, 3, rcond=None, full=False, w=None, cov=False))
self.fit_y2_para = list(np.polyfit(fit_x, fit_y2, 3, rcond=None, full=False, w=None, cov=False))
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)
def __init__(
self,
training_task, # 'left', 'straight', 'right'
num_future_data=0,
display=False,
**kwargs):
metadata = {'render.modes': ['human']}
self.dynamics = VehicleDynamics()
self.interested_vehs = None
self.training_task = training_task
self.ref_path = ReferencePath(self.training_task, **kwargs)
self.detected_vehicles = None
self.all_vehicles = None
self.ego_dynamics = None
self.num_future_data = num_future_data
self.env_model = EnvironmentModel(training_task, num_future_data)
self.init_state = {}
self.action_number = 2
self.exp_v = EXPECTED_V #TODO: temp
self.ego_l, self.ego_w = L, W
self.action_space = gym.spaces.Box(low=-1,
high=1,
shape=(self.action_number, ),
dtype=np.float32)
self.seed()
self.v_light = None
self.step_length = 100 # ms
self.step_time = self.step_length / 1000.0
self.init_state = self._reset_init_state()
self.obs = None
self.action = None
self.veh_mode_dict = VEHICLE_MODE_DICT[self.training_task]
self.veh_num = VEH_NUM[self.training_task]
self.virtual_red_light_vehicle = False
self.done_type = 'not_done_yet'
self.reward_info = None
self.ego_info_dim = None
self.per_tracking_info_dim = None
self.per_veh_info_dim = None
if not display:
self.traffic = Traffic(self.step_length,
mode='training',
init_n_ego_dict=self.init_state,
training_task=self.training_task)
self.reset()
action = self.action_space.sample()
observation, _reward, done, _info = self.step(action)
self._set_observation_space(observation)
plt.ion()
def _reset_init_state():
ref_path = ReferencePath('straight')
random_index = int(np.random.random()*(900+500)) + 700
x, y, phi = ref_path.indexs2points(random_index)
v = 8 * np.random.random()
return dict(ego=dict(v_x=v,
v_y=0,
r=0,
x=x.numpy(),
y=y.numpy(),
phi=phi.numpy(),
l=4.8,
w=2.2,
routeID='du',
))
def reset(self, **kwargs): # kwargs include three keys
self.ref_path = ReferencePath(self.training_task, **kwargs)
self.init_state = self._reset_init_state()
self.traffic.init_traffic(self.init_state)
self.traffic.sim_step()
ego_dynamics = self._get_ego_dynamics([
self.init_state['ego']['v_x'], self.init_state['ego']['v_y'],
self.init_state['ego']['r'], self.init_state['ego']['x'],
self.init_state['ego']['y'], self.init_state['ego']['phi']
], [
0, 0, self.dynamics.vehicle_params['miu'],
self.dynamics.vehicle_params['miu']
])
self._get_all_info(ego_dynamics)
self.obs = self._get_obs()
self.action = None
self.reward_info = None
self.done_type = 'not_done_yet'
if np.random.random() > 0.9:
self.virtual_red_light_vehicle = True
else:
self.virtual_red_light_vehicle = False
return self.obs
def plot_static_path():
square_length = CROSSROAD_SIZE
extension = 20
lane_width = LANE_WIDTH
light_line_width = 3
dotted_line_style = '--'
solid_line_style = '-'
fig = plt.figure(figsize=(8, 8))
ax = plt.axes([0, 0, 1, 1])
for ax in fig.get_axes():
ax.axis('off')
ax.axis("equal")
# ----------arrow--------------
plt.arrow(lane_width / 2, -square_length / 2 - 10, 0, 5, color='b')
plt.arrow(lane_width / 2, -square_length / 2 - 10 + 5, -0.5, 0, color='b', head_width=1)
plt.arrow(lane_width * 1.5, -square_length / 2 - 10, 0, 4, color='b', head_width=1)
plt.arrow(lane_width * 2.5, -square_length / 2 - 10, 0, 5, color='b')
plt.arrow(lane_width * 2.5, -square_length / 2 - 10 + 5, 0.5, 0, color='b', head_width=1)
# ----------horizon--------------
plt.plot([-square_length / 2 - extension, -square_length / 2], [0.3, 0.3], color='orange')
plt.plot([-square_length / 2 - extension, -square_length / 2], [-0.3, -0.3], color='orange')
plt.plot([square_length / 2 + extension, square_length / 2], [0.3, 0.3], color='orange')
plt.plot([square_length / 2 + extension, square_length / 2], [-0.3, -0.3], color='orange')
for i in range(1, LANE_NUMBER + 1):
linestyle = dotted_line_style if i < LANE_NUMBER else solid_line_style
linewidth = 1 if i < LANE_NUMBER else 2
plt.plot([-square_length / 2 - extension, -square_length / 2], [i * lane_width, i * lane_width],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([square_length / 2 + extension, square_length / 2], [i * lane_width, i * lane_width],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([-square_length / 2 - extension, -square_length / 2], [-i * lane_width, -i * lane_width],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([square_length / 2 + extension, square_length / 2], [-i * lane_width, -i * lane_width],
linestyle=linestyle, color='black', linewidth=linewidth)
# ----------vertical----------------
plt.plot([0.3, 0.3], [-square_length / 2 - extension, -square_length / 2], color='orange')
plt.plot([-0.3, -0.3], [-square_length / 2 - extension, -square_length / 2], color='orange')
plt.plot([0.3, 0.3], [square_length / 2 + extension, square_length / 2], color='orange')
plt.plot([-0.3, -0.3], [square_length / 2 + extension, square_length / 2], color='orange')
for i in range(1, LANE_NUMBER + 1):
linestyle = dotted_line_style if i < LANE_NUMBER else solid_line_style
linewidth = 1 if i < LANE_NUMBER else 2
plt.plot([i * lane_width, i * lane_width], [-square_length / 2 - extension, -square_length / 2],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([i * lane_width, i * lane_width], [square_length / 2 + extension, square_length / 2],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([-i * lane_width, -i * lane_width], [-square_length / 2 - extension, -square_length / 2],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([-i * lane_width, -i * lane_width], [square_length / 2 + extension, square_length / 2],
linestyle=linestyle, color='black', linewidth=linewidth)
plt.plot([0, LANE_NUMBER * lane_width], [-square_length / 2, -square_length / 2],
color='black', linewidth=light_line_width, alpha=0.3)
plt.plot([LANE_NUMBER * lane_width, 0], [square_length / 2, square_length / 2],
color='black', linewidth=light_line_width, alpha=0.3)
plt.plot([-square_length / 2, -square_length / 2], [0, LANE_NUMBER * lane_width],
color='black', linewidth=light_line_width, alpha=0.3)
plt.plot([square_length / 2, square_length / 2], [-LANE_NUMBER * lane_width, 0],
color='black', linewidth=light_line_width, alpha=0.3)
# ----------Oblique--------------
plt.plot([LANE_NUMBER * lane_width, square_length / 2], [-square_length / 2, -LANE_NUMBER * lane_width],
color='black', linewidth=2)
plt.plot([LANE_NUMBER * lane_width, square_length / 2], [square_length / 2, LANE_NUMBER * lane_width],
color='black', linewidth=2)
plt.plot([-LANE_NUMBER * lane_width, -square_length / 2], [-square_length / 2, -LANE_NUMBER * lane_width],
color='black', linewidth=2)
plt.plot([-LANE_NUMBER * lane_width, -square_length / 2], [square_length / 2, LANE_NUMBER * lane_width],
color='black', linewidth=2)
for task in ['left', 'straight', 'right']:
path = ReferencePath(task)
path_list = path.path_list
control_points = path.control_points
color = ['blue', 'coral', 'darkcyan']
for i, (path_x, path_y, _) in enumerate(path_list):
plt.plot(path_x[600:-600], path_y[600:-600], color=color[i])
for i, four_points in enumerate(control_points):
for point in four_points:
plt.scatter(point[0], point[1], color=color[i])
plt.plot([four_points[0][0], four_points[1][0]], [four_points[0][1], four_points[1][1]], linestyle='--', color=color[i])
plt.plot([four_points[1][0], four_points[2][0]], [four_points[1][1], four_points[2][1]], linestyle='--', color=color[i])
plt.plot([four_points[2][0], four_points[3][0]], [four_points[2][1], four_points[3][1]], linestyle='--', color=color[i])
plt.savefig('./multipath_planning.pdf')
plt.show()