def control_host(host_vehicle):
""" a controller control the host, if emergency event(Collision) would be happen, it would prevent it."""
def pd_control_vel(velocity, target_velocity, last_error, error_sum):
"""a simple PID controller of host vehicle for coliision purpose"""
k_p = 1.
k_d = 0.5
k_i = 0.05
error = target_velocity - velocity
error_sum += error
d_error = error - last_error
error_sum = min(error_sum, 20)
throttle = k_p * error + k_d * d_error + k_i * error_sum
# print('throttle- ', error_sum)
return max(min(throttle, 1.), 0.), error, error_sum
def pd_control_for_safety(degree_likelihood, target_likelihood, last_error,
error_sum, kp, kd, ki):
"""aa
Return:
if val > 0, brake,
else, throttle
"""
# k_p = 4
# k_d = 2.5
k_p = kp
k_d = kd
k_i = ki
error = target_likelihood - degree_likelihood
error_sum += error
d_error = error - last_error
val = k_p * error + k_d * d_error + k_i * error_sum
return max(min(val, 1.), -1.), error, error_sum
global stop
last_error_pdcc = 0.
error_sum_pdcc = 0.
last_error_pdcs = 0.
error_sum_pdcs = 0.
time_step = 0
emergency_control = False
normal_drive = True
thread_record_d = []
thread_record_a = []
thread_record_s = []
comfort_record_m = []
comfort_record_a = []
comfort_record_c = []
vel_record = []
acc_record = []
relative_distance_record = []
ttc_record = []
other = get_other(world)
record_start_step = 50
while True:
## record info
pos = host_vehicle.get_location()
velocity = host_vehicle.get_velocity()
velocity_ = math.sqrt(velocity.x**2 + velocity.y**2)
# print('host, ', velocity_)
# vel_record.append([time_step, velocity_])
acc = host_vehicle.get_acceleration()
# acc_record.append([time_step, -acc.x])
if time_step >= record_start_step:
vel_record.append([time_step - record_start_step, velocity_])
acc_record.append([time_step - record_start_step, acc.x])
comfort_scores = comfort_level_scores(acc.x)
if time_step >= record_start_step:
comfort_record_m.append(
[time_step - record_start_step, comfort_scores[0]])
comfort_record_a.append(
[time_step - record_start_step, comfort_scores[1]])
comfort_record_c.append(
[time_step - record_start_step, comfort_scores[2]])
other_pos = other.get_location()
other_velocity = other.get_velocity()
if time_step >= record_start_step:
relative_distance_record.append(
[time_step - record_start_step,
pos.distance(other_pos)])
## assess the situation
ego_v_state = ego_v.ego_vehicle()
ego_v_state.set_position(position=(pos.x, pos.y, pos.z))
ego_v_state.set_linear(linear=(velocity.x, velocity.y, velocity.z))
ego_v_state.set_size(size=(1.6, 1.6, 3.2))
road_obj_state = road_o.road_obj()
road_obj_state.set_position(position=(other_pos.x, other_pos.y,
other_pos.z))
road_obj_state.set_linear(linear=(other_velocity.x, other_velocity.y,
other_velocity.z))
road_obj_state.set_size(size=(1.6, 1.6, 3.2))
score, ttc = _assess_one_obj_threat_score(ego_v_state, road_obj_state)
degree = _score_2_threat_degree(score)
# print(str(score) + '---'+str(degree))
if time_step >= record_start_step:
ttc_record.append([time_step - record_start_step, ttc])
thread_record_d.append([time_step - record_start_step, score[0]])
thread_record_a.append([time_step - record_start_step, score[1]])
thread_record_s.append([time_step - record_start_step, score[2]])
if normal_drive:
throttle, last_error_pdcc, error_sum_pdcc = pd_control_vel(
velocity_, config.host_target_vel_3, last_error_pdcc,
error_sum_pdcc)
control = carla.VehicleControl(throttle=throttle)
host_vehicle.apply_control(control)
if emergency_control:
# ## strategy-1 : emergency braking
if strategy_type == STRATEGY.AGGRESSIVE:
if degree == safety_degree.dangerous and score[0] >= 0.8:
# print('brake!!')
control = carla.VehicleControl(brake=1.)
host_vehicle.apply_control(control)
elif degree == safety_degree.safe and score[2] >= 0.8:
control = carla.VehicleControl(throttle=0.35)
host_vehicle.apply_control(control)
else:
control = carla.VehicleControl(throttle=0., brake=1.)
host_vehicle.apply_control(control)
## strategy-1 : emergency braking
## strategy-2 : pd control
elif strategy_type == STRATEGY.CONSERVATIVE:
# print('aa')
val, last_error_pdcs, error_sum_pdcs = pd_control_for_safety(
score[2],
0.8,
last_error_pdcs,
error_sum_pdcs,
kp=1.,
kd=0.1,
ki=0.)
# print('brake -- ', val)
if val >= 0:
print('brake -- ', val)
control = carla.VehicleControl(brake=val)
else:
control = carla.VehicleControl(throttle=0.3)
## strategy-3 : pd control
elif strategy_type == STRATEGY.NORMAL:
val, last_error_pdcs, error_sum_pdcs = pd_control_for_safety(
min(score[0] + score[1], 1.),
0.5,
last_error_pdcs,
error_sum_pdcs,
kp=2.,
kd=0.5,
ki=0.)
# print(val)
if val <= 0:
brake = math.fabs(val)
print('brake -- ', brake)
control = carla.VehicleControl(brake=brake)
else:
control = carla.VehicleControl(throttle=0.35)
else:
raise ValueError('strategy_type wrong...')
host_vehicle.apply_control(control)
if pos.x > 45.:
logger.info('Stop test...')
stop = True
break
## transform the control state
## for strategy-1
if strategy_type == STRATEGY.AGGRESSIVE:
if collision_avoidance and degree == safety_degree.dangerous and score[
0] >= 0.8:
pd_control = False
emergency_control = True
## for strategy-2
elif strategy_type == STRATEGY.CONSERVATIVE:
if collision_avoidance and score[2] <= 0.8:
pd_control = False
emergency_control = True
## for strategy-3
elif strategy_type == STRATEGY.NORMAL:
if collision_avoidance and degree == safety_degree.attentive and score[
1] >= 0.5:
pd_control = False
emergency_control = True
#
#
# # logger.info('threat degree for other vehicle:'+str(degree))
# ## assess the situation
#
# ## control
# if collision_avoidance:
# if not emergency_control:
# throttle, last_error = pd_control_for_collision(velocity, distance_collision, last_error)
# control = carla.VehicleControl(throttle=throttle)
# else:
# control = carla.VehicleControl(brake=1.)
# if velocity_ < 0.01:
# logger.info('Stop testing...')
# i = 0
# while (i<20):
# thread_record_d.append([time_step, 0])
# thread_record_a.append([time_step, 0])
# thread_record_s.append([time_step, 1])
#
# comfort_record_m.append([time_step, 0])
# comfort_record_a.append([time_step, 0])
# comfort_record_c.append([time_step, 1])
#
# vel_record.append([time_step, velocity_])
# acc_record.append([time_step, -acc.x])
#
# pos = get_host(world).get_location()
# other_pos = get_other(world).get_location()
# relative_distance_record.append([time_step, pos.distance(other_pos)])
#
# time_step += 1
# i += 1
# time.sleep(0.1)
# break
# else:
# throttle, last_error = pd_control_for_collision(velocity, distance_collision, last_error)
# control = carla.VehicleControl(throttle=throttle)
# host_vehicle.apply_control(control)
# if distance_collision > 10:
# control = carla.VehicleControl(throttle=1.)
# host_vehicle.apply_control(control)
# else:
# control = carla.VehicleControl(brake=1.)
# host_vehicle.apply_control(control)
time.sleep(0.05)
time_step += 1
if save_experiment_data:
plot_threat_curve(thread_record_d, thread_record_a, thread_record_s)
plot_vel_acc_rdis(vel_record, acc_record, relative_distance_record)
plot_comfort_curve(comfort_record_m, comfort_record_a,
comfort_record_c)
save_dict = {
'experiment_name': experiment_name,
'thread_record_d': thread_record_d,
'thread_record_a': thread_record_a,
'thread_record_s': thread_record_s,
'vel_record': vel_record,
'acc_record': acc_record,
'relative_distance_record': relative_distance_record,
'comfort_record_m': comfort_record_m,
'comfort_record_a': comfort_record_a,
'comfort_record_c': comfort_record_c,
'ttc_record': ttc_record
}
with open(save_experiment_data_to, 'wb') as f:
pickle.dump(save_dict, f)
logger.info('save success... file - %s' %
(save_experiment_data_to))
def control_host(host_vehicle):
""" a controller control the host, if emergency event(Collision) would be happen, it would prevent it."""
def pd_control_for_collision(velocity, distance_collision_host, last_error):
"""a simple PD controller of host vehicle for coliision purpose"""
k_p = 1
k_d = 0.5
other = get_other(world)
other_pos = other.get_location()
distance_collision = other_pos.distance(carla.Location(x=-77.5, y=-3., z=pos.z))
o_velocity = other.get_velocity()
o_v = math.sqrt(o_velocity.x**2+o_velocity.y**2)
ttc = distance_collision/(o_v+1e-10)
target_v = distance_collision_host/(ttc+1e-10)
error = target_v - math.sqrt(velocity.x**2+velocity.y**2)
d_error = error - last_error
throttle = k_p*error + k_d*d_error
return max(min(throttle, 1.), 0.), error
def pd_control_for_safety(degree_likelihood, target_likelihood, last_error, kp, kd):
"""aa
Return:
if val > 0, brake,
else, throttle
"""
# k_p = 4
# k_d = 2.5
k_p = kp
k_d = kd
error = target_likelihood - degree_likelihood
d_error = error - last_error
val = k_p*error + k_d*d_error
return max(min(val, 1.), -1.), error
last_error_pdcc = 0.
last_error_pdcs = 0.
time_step = 0
emergency_control = False
pd_control = True
thread_record_d = []
thread_record_a = []
thread_record_s = []
comfort_record_m = []
comfort_record_a = []
comfort_record_c = []
vel_record = []
acc_record = []
relative_distance_record = []
other = get_other(world)
record_start_step = 50
while True:
## record info
pos = host_vehicle.get_location()
velocity = host_vehicle.get_velocity()
velocity_ = math.sqrt(velocity.x**2 + velocity.y**2)
# vel_record.append([time_step, velocity_])
acc = host_vehicle.get_acceleration()
# acc_record.append([time_step, -acc.x])
if time_step >= record_start_step:
vel_record.append([time_step - record_start_step, velocity_])
acc_record.append([time_step - record_start_step, -acc.x])
comfort_scores = comfort_level_scores(-acc.x)
if time_step >= record_start_step:
comfort_record_m.append([time_step - record_start_step, comfort_scores[0]])
comfort_record_a.append([time_step - record_start_step, comfort_scores[1]])
comfort_record_c.append([time_step - record_start_step, comfort_scores[2]])
other_pos = other.get_location()
other_velocity = other.get_velocity()
if time_step >= record_start_step:
relative_distance_record.append([time_step - record_start_step, pos.distance(other_pos)])
# logger.info('host_vehicle velocity:' + str(velocity) + ' acc:' + str(acc))
distance_collision = pos.distance(carla.Location(x=-77.5, y=-3., z=pos.z))
# logger.info('remaining distance:' + str(distance_collision))
## assess the situation
ego_v_state = ego_v.ego_vehicle()
ego_v_state.set_position(position=(pos.x, pos.y, pos.z))
ego_v_state.set_linear(linear=(velocity.x, velocity.y, velocity.z))
ego_v_state.set_size(size=(1.6, 1.6, 3.2))
road_obj_state = road_o.road_obj()
road_obj_state.set_position(position=(other_pos.x, other_pos.y, other_pos.z))
road_obj_state.set_linear(linear=(other_velocity.x, other_velocity.y, other_velocity.z))
road_obj_state.set_size(size=(1.6, 1.6, 3.2))
score = _assess_one_obj_threat_score(ego_v_state, road_obj_state)
degree = _score_2_threat_degree(score)
if time_step >= record_start_step:
thread_record_d.append([time_step - record_start_step, score[0]])
thread_record_a.append([time_step - record_start_step, score[1]])
thread_record_s.append([time_step - record_start_step, score[2]])
if pd_control:
throttle, last_error_pdcc = pd_control_for_collision(velocity, distance_collision, last_error_pdcc)
control = carla.VehicleControl(throttle=throttle)
host_vehicle.apply_control(control)
if emergency_control:
# ## strategy-1 : emergency braking
if strategy_type == STRATEGY.AGGRESSIVE:
if degree == safety_degree.dangerous and score[0] >= 0.8:
# print('brake!!')
control = carla.VehicleControl(brake=1.)
host_vehicle.apply_control(control)
elif degree == safety_degree.safe and score[2] >= 0.8:
control = carla.VehicleControl(throttle=1.)
host_vehicle.apply_control(control)
else:
control = carla.VehicleControl(brake=1.)
host_vehicle.apply_control(control)
## strategy-1 : emergency braking
## strategy-2 : pd control
elif strategy_type == STRATEGY.CONSERVATIVE:
val, last_error_pdcs = pd_control_for_safety(score[2], 0.8, last_error_pdcs, kp=0.1, kd=0.1)
if val >= 0:
control = carla.VehicleControl(brake=val)
else:
control = carla.VehicleControl(throttle=min(1., math.fabs(val)+0.5))
## strategy-3 : pd control
elif strategy_type == STRATEGY.NORMAL:
val, last_error_pdcs = pd_control_for_safety(score[1], 0.5, last_error_pdcs, kp=2, kd=0.5)
if val <= 0:
control = carla.VehicleControl(brake=math.fabs(val))
else:
control = carla.VehicleControl(throttle=val)
else:
raise ValueError('strategy_type wrong...')
host_vehicle.apply_control(control)
if pos.x < -85:
logger.info('Stop test...')
break
## transform the control state
## for strategy-1
if strategy_type == STRATEGY.AGGRESSIVE:
if collision_avoidance and degree == safety_degree.dangerous and score[0] >= 0.8:
pd_control = False
emergency_control = True
## for strategy-2
elif strategy_type == STRATEGY.CONSERVATIVE:
if collision_avoidance and degree == safety_degree.safe and score[2] <= 0.8:
pd_control = False
emergency_control = True
## for strategy-3
elif strategy_type == STRATEGY.NORMAL:
if collision_avoidance and degree == safety_degree.attentive and score[1] >= 0.5:
pd_control = False
emergency_control = True
#
#
# # logger.info('threat degree for other vehicle:'+str(degree))
# ## assess the situation
#
# ## control
# if collision_avoidance:
# if not emergency_control:
# throttle, last_error = pd_control_for_collision(velocity, distance_collision, last_error)
# control = carla.VehicleControl(throttle=throttle)
# else:
# control = carla.VehicleControl(brake=1.)
# if velocity_ < 0.01:
# logger.info('Stop testing...')
# i = 0
# while (i<20):
# thread_record_d.append([time_step, 0])
# thread_record_a.append([time_step, 0])
# thread_record_s.append([time_step, 1])
#
# comfort_record_m.append([time_step, 0])
# comfort_record_a.append([time_step, 0])
# comfort_record_c.append([time_step, 1])
#
# vel_record.append([time_step, velocity_])
# acc_record.append([time_step, -acc.x])
#
# pos = get_host(world).get_location()
# other_pos = get_other(world).get_location()
# relative_distance_record.append([time_step, pos.distance(other_pos)])
#
# time_step += 1
# i += 1
# time.sleep(0.1)
# break
# else:
# throttle, last_error = pd_control_for_collision(velocity, distance_collision, last_error)
# control = carla.VehicleControl(throttle=throttle)
# host_vehicle.apply_control(control)
# if distance_collision > 10:
# control = carla.VehicleControl(throttle=1.)
# host_vehicle.apply_control(control)
# else:
# control = carla.VehicleControl(brake=1.)
# host_vehicle.apply_control(control)
time.sleep(0.05)
time_step += 1
# thread_record_d = thread_record_d[len(thread_record_d)//5:]
# thread_record_a = thread_record_a[len(thread_record_a) // 5:]
# thread_record_s = thread_record_s[len(thread_record_s) // 5:]
#
# vel_record = vel_record[len(vel_record) // 5:]
# acc_record = acc_record[len(acc_record) // 5:]
# relative_distance_record = relative_distance_record[len(relative_distance_record) // 5:]
#
# comfort_record_m = comfort_record_m[len(comfort_record_m) // 5:]
# comfort_record_a = comfort_record_a[len(comfort_record_a) // 5:]
# comfort_record_c = comfort_record_m[len(comfort_record_m) // 5:]
plot_threat_curve(thread_record_d, thread_record_a, thread_record_s)
plot_vel_acc_rdis(vel_record, acc_record, relative_distance_record)
plot_comfort_curve(comfort_record_m, comfort_record_a, comfort_record_c)
if save_experiment_data:
save_dict = {'experiment_name': experiment_name,
'thread_record_d': thread_record_d,
'thread_record_a': thread_record_a,
'thread_record_s': thread_record_s,
'vel_record': vel_record,
'acc_record': acc_record,
'relative_distance_record': relative_distance_record,
'comfort_record_m': comfort_record_m,
'comfort_record_a': comfort_record_a,
'comfort_record_c': comfort_record_c}
with open(save_experiment_data_to, 'wb') as f:
pickle.dump(save_dict, f)
logger.info('save success... file - %s'%(save_experiment_data_to))
weather_type])]
dangerous_score = gaussian_1d(x=ttc,
mean=thresh[0], for_what=safety_degree.dangerous,
std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2)
attentive_score = gaussian_1d(x=ttc,
mean=thresh[1], for_what=safety_degree.attentive,
std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2)
safe_score = gaussian_1d(x=ttc,
mean=thresh[2], for_what=safety_degree.safe,
std=(ego_vel_scalar * 3.6) / 20, max=(ego_vel_scalar * 3.6) / 2)
return softmax(np.array([dangerous_score, attentive_score, safe_score])), ttc
else:
raise ValueError("Imposible get here!!! Something must wrong!!!")
if __name__ == '__main__':
ego_v_state = ego_v.ego_vehicle()
ego_v_state.set_position(position=(0., 0., 0.))
ego_v_state.set_linear(linear=(15., 1., 0.))
ego_v_state.set_size(size=(1.3, 1.5, 4.))
road_obj_state = road_o.road_obj()
road_obj_state.set_position(position=(50., 0., 0.))
road_obj_state.set_linear(linear=(1., 0., 0.))
road_obj_state.set_size(size=(1.3, 1.5, 4.))
safety_degree, ttc = _assess_one_obj_safety(ego_vehicle=ego_v_state, road_obj=road_obj_state)
pass
def produce_heat_map(ego,
others,
h_type,
hm_size=(224, 224),
consider_range=50):
"""produce heat map for each safety degree
Args:
ego: ego vehecle in carla
others: other actor in carla
"""
assert h_type in ['danger', 'attentive', 'safe']
ego_location = ego.get_location()
ego_location = (ego_location.x, ego_location.y, ego_location.z)
ego_size = ego.bounding_box.extent
ego_size = (ego_size.x, ego_size.y, ego_size.z)
ego_velocity = ego.get_velocity()
ego_velocity = (ego_velocity.x, ego_velocity.y, ego_velocity.z)
t = ego.get_transform()
f_v = t.get_forward_vector()
cos_theta = (f_v.x * 0 + f_v.y * 1) / math.sqrt(f_v.x**2 + f_v.y**2)
a = math.acos(cos_theta)
if f_v.x > 0:
a = -a
r_matix = np.array([[math.cos(a), -math.sin(a)],
[math.sin(a), math.cos(a)]])
# points = []
# sizes = []
hms = []
for vehicle in others:
location = vehicle.get_location()
location = (location.x, location.y, location.z)
distance = math.sqrt((ego_location[0] - location[0])**2 +
(ego_location[1] - location[1])**2)
if distance <= consider_range:
size = vehicle.bounding_box.extent
size = (size.x, size.y, size.z)
velocity = vehicle.get_velocity()
velocity = (velocity.x, velocity.y, velocity.z)
ego_v_state = ego_v.ego_vehicle()
ego_v_state.set_position(position=ego_location)
ego_v_state.set_linear(linear=ego_velocity)
ego_v_state.set_size(size=ego_size)
road_obj_state = road_o.road_obj()
road_obj_state.set_position(position=location)
road_obj_state.set_linear(linear=velocity)
road_obj_state.set_size(size=size)
safety_degree = _assess_one_obj_threat_score(
ego_vehicle=ego_v_state, road_obj=road_obj_state)
max_index = np.argmax(np.array(safety_degree))
if max_index == ['danger', 'attentive', 'safe'].index(h_type):
relative_x = int(location[0] - ego_location[0]) * (
hm_size[1] // consider_range // 2)
relative_y = int(location[1] - ego_location[1]) * (
hm_size[0] // consider_range // 2)
point = np.matmul(np.array([relative_x, relative_y]), r_matix)
point_x = min(hm_size[1] - 1,
max(-point[0] + hm_size[1] // 2, 0))
point_y = min(hm_size[0] - 1,
max(-point[1] + hm_size[0] // 2, 0))
size = vehicle.bounding_box.extent
size = math.sqrt(size.x**2 + size.y**2)
# points.append([point_x, point_y])
# sizes.append(size/3)
hm = heat_map(hm_size,
points=[[point_x, point_y]],
sigma=size * 2)
hm *= safety_degree[max_index]
hms.append(hm)
if len(hms) > 0:
hm = np.sum(np.array(hms), axis=0)
return hm
else:
return np.zeros(hm_size)