def predict_step_with_ego(self, selected_speed, delta_t, min_crash_distance=5):
current_x, current_y = self.ego_position
if current_x < control.merge_point2[0]:
# Approximate the chane in position as traveling straight towards the merge point
direction = np.array([control.merge_point2[0] - current_x, control.merge_point2[1] - current_y])
direction /= np.linalg.norm(direction)
direction *= selected_speed * delta_t
predicted_x = current_x + direction[0]
predicted_y = current_y + direction[1]
if predicted_y < -1.6:
predicted_y = -1.6 # The lane to merge into is at y=-1.6
else:
predicted_y = current_y
predicted_x = current_x + selected_speed * delta_t
next_acceleration = (selected_speed - self.ego_speed) / delta_t
# The ego can't crash into other cars until this point. Not 100% sure if accurate
ego_can_crash = control.get_ego_s((predicted_x, predicted_y)) > self.ego_crash_threshold
# Also don't let the other cars really react to the ego car until a threshold
ego_has_merged = control.get_ego_s((predicted_x, predicted_y)) > self.ego_reaction_threshold
# The other cars are in the order from front to back
new_other_xs = []
new_other_speeds = []
new_other_accelerations = []
last_x = np.inf
last_speed = 0
ego_encountered = False
for other_car_index in range(len(self.other_xs)):
other_speed = self.other_speeds[other_car_index]
other_x = self.other_xs[other_car_index]
if other_x < predicted_x and not ego_encountered:
ego_encountered = True
if ego_has_merged:
last_x = predicted_x
last_speed = selected_speed
speed_diff = last_speed - other_speed
x_diff = last_x - other_x
if speed_diff < 0 and x_diff < 30:
new_other_acceleration = max(speed_diff, Settings.MAX_PREDICTED_DECELERATION)
new_other_speed = other_speed + new_other_acceleration * delta_t
else:
new_other_acceleration = 0
new_other_speed = other_speed
predicted_next_position = other_x + new_other_speed * delta_t
last_x = predicted_next_position
last_speed = new_other_speed
new_other_xs.append(predicted_next_position)
new_other_speeds.append(new_other_speed)
new_other_accelerations.append(new_other_acceleration)
crashed = False
crash_detection_distance = max(Settings.CAR_LENGTH, min_crash_distance)
for x in new_other_xs:
if abs(x - predicted_x) < crash_detection_distance and ego_can_crash:
crashed = True
return HighwayState((predicted_x, predicted_y), selected_speed, next_acceleration, new_other_xs, new_other_speeds, new_other_accelerations), crashed
def st_reward(state, jerk, crashed, arrived):
absolute_metric = 0
speed_metric = 0
acceleration_metric = 0
jerk_metric = 0
distance_metric = 0
if crashed:
absolute_metric = -10
elif arrived:
absolute_metric = 10
else:
jerk_metric = -jerk**2 * Settings.TICK_LENGTH
ego_speed = state.ego_speed
car_ahead, car_behind = state.get_closest_cars()
ego_acceleration = state.ego_acceleration
ego_position = state.ego_position
ego_x, ego_y = ego_position
ego_s = control.get_ego_s(ego_position)
speed_metric = -Settings.TICK_LENGTH * (ego_speed -
Settings.DESIRED_SPEED)**2
acceleration_metric = -Settings.TICK_LENGTH * ego_acceleration**2
if ego_s > 0:
if car_ahead is not None:
ahead_x, ahead_speed, ahead_acceleration = car_ahead
front_distance = ahead_x - ego_x - Settings.CAR_LENGTH
else:
front_distance = np.inf
if car_behind is not None:
behind_x, behind_speed, behind_acceleration = car_behind
back_distance = ego_x - behind_x - Settings.CAR_LENGTH
else:
back_distance = np.inf
min_distance = min(front_distance, back_distance)
if min_distance < Settings.MIN_FOLLOW_DISTANCE:
distance_metric = -2 / max(min_distance, 1)
elif min_distance == np.inf:
distance_metric = 0
elif np.isnan(min_distance):
distance_metric = 0
else:
distance_metric = -1 / min_distance
distance_metric *= Settings.TICK_LENGTH
return Settings.ALT_A_WEIGHT * acceleration_metric + Settings.ALT_D_WEIGHT * distance_metric + \
Settings.ALT_J_WEIGHT * jerk_metric + Settings.ALT_V_WEIGHT * speed_metric + absolute_metric
def continuous_reward(state: prediction.HighwayState, jerk, crashed, arrived):
absolute_metric = 0
safety_metric = 0
efficiency_metric = 0
smoothness_metric = 0
if crashed:
absolute_metric = -10
elif arrived:
absolute_metric = 10
else:
smoothness_metric = -abs(jerk) * Settings.TICK_LENGTH
ego_speed = state.ego_speed
car_ahead, car_behind = state.get_closest_cars()
ego_position = state.ego_position
ego_x, ego_y = ego_position
ego_s = control.get_ego_s(ego_position)
if ego_s > 0:
if car_ahead is not None:
ahead_x, ahead_speed, ahead_acceleration = car_ahead
front_distance = ahead_x - ego_x - Settings.CAR_LENGTH
else:
front_distance = np.inf
if car_behind is not None:
behind_x, behind_speed, behind_acceleration = car_behind
back_distance = ego_x - behind_x - Settings.CAR_LENGTH
else:
back_distance = np.inf
min_distance = min(front_distance, back_distance)
if min_distance < Settings.MIN_FOLLOW_DISTANCE:
safety_metric = -1
elif min_distance == np.inf:
safety_metric = 0
elif np.isnan(min_distance):
safety_metric = 0
else:
safety_metric = -1 / min_distance
safety_metric *= Settings.TICK_LENGTH
efficiency_metric = -Settings.TICK_LENGTH * np.abs(
ego_speed - Settings.DESIRED_SPEED)
return Settings.WT_SMOOTH * smoothness_metric + Settings.WT_SAFE * safety_metric + Settings.WT_EFFICIENT * efficiency_metric + absolute_metric
def predict_step_without_ego(self, delta_t, min_crash_distance=5):
# Assume ego will just follow the car in front as closely as possible if ego is merge, or ego just stays put otherwise
ego_s = control.get_ego_s(self.ego_position)
ego_x = self.ego_position[0]
if ego_s < self.ego_reaction_threshold or len(self.other_xs) == 0:
return self.predict_step_with_ego(0, delta_t, min_crash_distance)
elif self.other_xs[0] < ego_x:
# If ego car is in front and on highway, pretend it's not there
modified_state = HighwayState((-20, -10), 0, 0, self.other_xs, self.other_speeds, self.other_accelerations)
return modified_state.predict_step_with_ego(0, delta_t, min_crash_distance)
else:
last_speed = 0
last_x = 0
for i, car_x in enumerate(self.other_xs):
if car_x < ego_x:
# Assume the ego car directly follows the car in front of it for the purposes of allocating
# space to other vehicles when predicting motion
modified_state = HighwayState((last_x - Settings.CAR_LENGTH - 5, self.ego_position[1]), last_speed, 0, self.other_xs, self.other_speeds, self.other_accelerations)
return modified_state.predict_step_with_ego(last_speed, delta_t, min_crash_distance)
else:
last_speed = self.other_speeds[i]
last_x = car_x
return self.predict_step_with_ego(last_speed, delta_t, min_crash_distance)
def get_s_state():
return control.get_ego_s(control.get_ego_position())
def find_s_t_obstacles_from_state(current_state,
future_s=150,
delta_s=0.5,
delta_t=0.2,
time_limit=5,
start_uncertainty=0.0,
uncertainty_per_second=0.1):
ego_position = current_state.ego_position
ego_speed = current_state.ego_speed
start_s = control.get_ego_s(ego_position)
# We discretize the s and t space, and store the lookup for s and t values in these arrays
s_values = np.arange(start_s, start_s + future_s + delta_s, delta_s)
t_values = np.arange(0, time_limit + delta_t, delta_t)
obstacles = np.zeros((t_values.size, s_values.size), dtype=np.bool)
distances = np.zeros(obstacles.shape, dtype=np.float)
distances += 1E10 # Big number but not NaN
discrete_length = int(Settings.CAR_LENGTH / delta_s)
predicted_state = current_state
for (t_index, t) in enumerate(t_values):
uncertainty = start_uncertainty + uncertainty_per_second * t
discrete_uncertainty = int(uncertainty / delta_s)
if t_index != 0:
predicted_state, _ = predicted_state.predict_step_without_ego(
delta_t)
for vehicle_index, vehicle_x in enumerate(predicted_state.other_xs):
current_obstacle_s = control.get_obstacle_s_from_x(vehicle_x)
if current_obstacle_s < Settings.CRASH_MIN_S - Settings.MIN_ALLOWED_DISTANCE:
break # Cars do not obstruct path until the merge point
elif current_obstacle_s > s_values[-1] + Settings.CAR_LENGTH:
continue
# calculate the distance from each point to this obstacle vehicle at time t
obstacle_distances_front = np.abs(s_values - (
current_obstacle_s - Settings.CAR_LENGTH - uncertainty))
obstacle_distances_back = np.abs(s_values - (
current_obstacle_s + Settings.CAR_LENGTH + uncertainty))
# the distance is the minimum of the distance from the front of the ego vehicle to the back of the
# obstacle vehicle, and from the front of the obstacle to the back of the ego vehicle
distances[t_index] = np.minimum(obstacle_distances_front,
distances[t_index, :])
distances[t_index] = np.minimum(obstacle_distances_back,
distances[t_index, :])
# Within a vehicle length of the obstacle's s position, register the presence of an obstacle
start_s_index = get_range_index(start_s, delta_s,
current_obstacle_s)
index_min = max(
start_s_index - discrete_length - discrete_uncertainty, 0)
index_max = min(
start_s_index + discrete_length + discrete_uncertainty,
s_values.size)
if index_min < s_values.size and index_max > 0:
obstacles[t_index, index_min:index_max] = True
distances[t_index, index_min:index_max] = 0
# plt.close()
# plot_s_t_obstacles(obstacles, s_values, t_values)
# plt.show()
return obstacles, s_values, t_values, ego_speed, distances
def do_combined_control(self, state: prediction.HighwayState) -> float:
start_state = state
first_action = self.get_control(state)
future_action = first_action
rollout_s_history = [control.get_ego_s(start_state.ego_position)]
crash_predicted = False
test_state = None
selected_speed = 0
last_choice_rl = True
if len(self.takeover_history) > 0 and self.takeover_history[-1]:
last_choice_rl = False
i = 0
while not (crash_predicted or i >= max(Settings.ROLLOUT_LENGTH, 1)):
i += 1
if i != 1:
future_action = self.get_control(state)
current_speed = state.ego_speed
current_acceleration = state.ego_acceleration
selected_speed = control.get_ego_speed_from_jerk(
current_speed, current_acceleration, future_action)
state, crash_predicted = state.predict_step_with_ego(
selected_speed, Settings.TICK_LENGTH,
Settings.COMBINATION_MIN_DISTANCE)
if i == Settings.ST_TEST_ROLLOUTS:
test_state = state
rollout_s_history.append(control.get_ego_s(state.ego_position))
if state.ego_position[0] > Settings.STOP_X:
break
if test_state is None:
test_state = state
if Settings.CHECK_ROLLOUT_CRASH and crash_predicted:
print("Crash predicted")
self.takeover_history.append(True)
return st.do_st_control(start_state)
elif Settings.LIMIT_DQN_SPEED and selected_speed > Settings.DESIRED_SPEED:
print("DDPG going too fast")
self.takeover_history.append(True)
return st.do_st_control(start_state)
elif Settings.TEST_ROLLOUT_STATE and st.test_guaranteed_crash_from_state(
test_state):
print("ST solver not happy with rollout state")
self.takeover_history.append(True)
return st.do_st_control(start_state)
elif Settings.TEST_ST_STRICTLY_BETTER:
s_sequence, obstacles, s_values, t_values, distances = st.get_appropriate_base_st_path_and_obstacles(
start_state)
end_point = len(s_sequence)
while s_sequence[end_point - 1] == 0:
end_point -= 1
s_sequence = s_sequence[:end_point]
if Settings.TICK_LENGTH < Settings.T_DISCRETIZATION:
s_sequence = st.finer_fit(s_sequence, Settings.TICK_LENGTH,
Settings.T_DISCRETIZATION,
start_state.ego_speed,
start_state.ego_acceleration)
# We are essentially on top of another car, so there's not much we can do anyway
if len(s_sequence) <= 1:
self.takeover_history.append(False)
return control.set_ego_jerk(first_action)
min_planning_length = min(len(s_sequence), len(rollout_s_history))
st_jerk = st.get_path_mean_abs_jerk(
s_sequence[:min_planning_length], start_state.ego_speed,
start_state.ego_acceleration, Settings.TICK_LENGTH)
rl_jerk = st.get_path_mean_abs_jerk(
rollout_s_history[:min_planning_length], start_state.ego_speed,
start_state.ego_acceleration, Settings.TICK_LENGTH)
st_distance = s_sequence[min_planning_length - 1] - s_sequence[0]
rl_distance = rollout_s_history[min_planning_length -
1] - rollout_s_history[0]
if last_choice_rl or not Settings.REMEMBER_LAST_CHOICE_FOR_SWITCHING_COMBINED:
if (st_jerk < rl_jerk
and st_distance > rl_distance) or rl_distance == 0:
print("ST Path deemed better")
self.takeover_history.append(True)
planned_distance_first_step = s_sequence[1] - s_sequence[0]
end_speed_first_step = planned_distance_first_step / (
Settings.TICK_LENGTH)
control.set_ego_speed(end_speed_first_step)
return end_speed_first_step
else:
self.takeover_history.append(False)
return control.set_ego_jerk(first_action)
else:
if rl_jerk < st_jerk and rl_distance > st_distance:
print("RL path deemed better")
self.takeover_history.append(False)
return control.set_ego_jerk(first_action)
else:
self.takeover_history.append(True)
planned_distance_first_step = s_sequence[1] - s_sequence[0]
end_speed_first_step = planned_distance_first_step / (
Settings.TICK_LENGTH)
control.set_ego_speed(end_speed_first_step)
return end_speed_first_step
else:
self.takeover_history.append(False)
return control.set_ego_jerk(first_action)