def try_spawn_random_vehicle_at(world, transform, autopilot=True):
blueprints = world.get_blueprint_library().filter('vehicle.*')
if not autopilot:
blueprints = world.get_blueprint_library().filter(
'vehicle.nissan.micra')
blueprints = [
x for x in blueprints
if int(x.get_attribute('number_of_wheels')) == 4
]
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if autopilot:
blueprint.set_attribute('role_name', 'autopilot')
else:
blueprint.set_attribute('role_name', 'egopilot')
vehicle = world.try_spawn_actor(blueprint, transform)
if (vehicle is not None) and (autopilot):
vehicle.set_autopilot(True)
logger.info('spawned a autopilot %r at %s' %
(vehicle.type_id, transform.location))
return True
elif (vehicle is not None) and (not autopilot):
vehicle.set_autopilot(False)
logger.info('spawned a egopilot %r at %s' %
(vehicle.type_id, transform.location))
return True
return False
def try_spawn_random_pedestrain_at(world, transform):
blueprints = world.get_blueprint_library().filter('walker*')
blueprint = random.choice(blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
blueprint.set_attribute('role_name', 'pedestrain')
pedestrain = world.try_spawn_actor(blueprint, transform)
if (pedestrain is not None):
logger.info('spawned a pedestrain %r at %s' %
(pedestrain.type_id, transform.location))
return True
return False
def random_noise_thread():
"""a thread random add vehicle ops noise"""
global random_drive
while True:
logger.info('Start random drive...')
random_drive = True
respawn_actors(world, egopilots)
for egopilot in egopilots:
egopilot.set_autopilot(False)
time.sleep(3) ## wait a moment
for egopilot in egopilots:
logger.info('Stop random drive...')
egopilot.set_autopilot(True)
random_drive = False
time.sleep(5)
def respawn_static_actors(world, actors):
"""re-spawn the static actors in the carla world
Args:
world: client.get_world()
actors:world.get_actors()
Example:
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
actor_list = world.get_actors()
vehicles = list(actor_list.filter('vehicle*'))
if carla_actors_static(vehicles):
respawn_static_actors(vehicles)
"""
for vehicle in actors:
if actor_static(vehicle):
spawn_points = list(world.get_map().get_spawn_points())
index = random.randint(0, (len(spawn_points)) - 1)
# index = 45
vehicle.set_transform(spawn_points[index])
logger.info('Respawn ' + str(vehicle) + ' in ' +
str(spawn_points[index]))
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))
if other_vehicle_pos_x - host_vehicle_pos_x > 0.4:
turn_flag = True
time.sleep(0.05)
if __name__ == '__main__':
import cv2
from carla_utils.sensor_ops import bgr_camera
stop = False
#### carla world init ####
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
logger.info('Carla connect success...')
logger.info('Carla world initing...')
world = client.get_world()
destroy_all_actors(world)
## vehicle blueprint
blueprints = world.get_blueprint_library().filter('*tesla')
blueprints = [
x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4
]
## host vehicle settings
host_vehicle_bp = random.choice(blueprints)
if host_vehicle_bp.has_attribute('color'):
# ## UPDATE OPS ##
# bn_ops = tf.get_collection(tf.GraphKeys.UPDATE_OPS)
# with tf.control_dependencies(bn_ops):
# optimizer = tf.train.AdamOptimizer(learning_rate=lr, epsilon=1e-8)
# grads_and_vars = optimizer.compute_gradients(loss)
# ## clip the gradients ##
# capped_gvs = [(tf.clip_by_value(grad, -5., 5.), var)
# for grad, var in grads_and_vars if grad != None]
# update_ops = optimizer.apply_gradients(capped_gvs, global_step=global_step)
init = tf.global_variables_initializer()
saver = tf.train.Saver(tf.global_variables())
ckpt = tf.train.get_checkpoint_state(FLAGS.checkpoint_dir)
logger.info('Build tensorflow graph finish...')
logger.info('Total trainable parameters:%s' % str(
np.sum([
np.prod(v.get_shape().as_list()) for v in tf.trainable_variables()
])))
#### carla world init ####
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
logger.info('Carla connect success...')
logger.info('Carla world initing...')
world = client.get_world()
destroy_all_actors(world)
## spawn vehicles in carla world
def update_thread(sess):
"""a thread used to train an actor net"""
update_event.wait()
logger.info('Begin update the actor...')
avg_clf_loss = 0.
avg_ops_loss = 0.
current_step = 0
while True:
memorys = memory_pool.get(batch_size=FLAGS.batch_size)
imgs = []
other_states = []
actions = []
scene_labels = []
# img = memorys[0][0]
# img = np.uint8((img+1.)*255./2)
# print('state:', memorys[0][1])
# print('action:', memorys[0][2])
#
# cv2.imshow('test', img)
# cv2.waitKey()
for memory in memorys:
imgs.append(memory[0])
other_states.append(memory[1])
actions.append(memory[2])
scene_labels.append(memory[3])
if current_step < 20000:
op, clf_l, ops_l, current_step = sess.run(
[update_ops, clf_loss, ops_loss, global_step],
feed_dict={
input: np.array(imgs),
other_state: np.array(other_states),
std_action: np.array(actions),
scene_label: np.array(scene_labels),
lr: FLAGS.learning_rate
})
# op, net_loss, current_step = sess.run([update_ops, clf_loss, global_step], feed_dict={input: np.array(imgs),
# scene_label:np.array(scene_labels),
# lr: FLAGS.learning_rate})
elif current_step < 40000:
op, clf_l, ops_l, current_step = sess.run(
[update_ops, clf_loss, ops_loss, global_step],
feed_dict={
input: np.array(imgs),
other_state: np.array(other_states),
std_action: np.array(actions),
scene_label: np.array(scene_labels),
lr: FLAGS.learning_rate / 10
})
# op, net_loss, current_step = sess.run([update_ops, clf_loss, global_step], feed_dict={input: np.array(imgs),
# scene_label: np.array(
# scene_labels),
# lr: FLAGS.learning_rate/10})
elif current_step < 3000000:
op, clf_l, ops_l, current_step = sess.run(
[update_ops, clf_loss, ops_loss, global_step],
feed_dict={
input: np.array(imgs),
other_state: np.array(other_states),
std_action: np.array(actions),
scene_label: np.array(scene_labels),
lr: FLAGS.learning_rate / 100
})
# op, net_loss, current_step = sess.run([update_ops, clf_loss, global_step], feed_dict={input: np.array(imgs),
# scene_label: np.array(
# scene_labels),
# lr: FLAGS.learning_rate/100})
else:
print(current_step)
raise ValueError('!!')
if FLAGS.f_log_step != None:
## caculate average loss ##
step = current_step % FLAGS.f_log_step
avg_ops_loss = (avg_ops_loss * step + ops_l) / (step + 1.)
avg_clf_loss = (avg_clf_loss * step + clf_l) / (step + 1.)
if step == FLAGS.f_log_step - 1:
logger.info(
'Step%s ops_loss:%s clf_loss:%s' %
(str(current_step), str(avg_ops_loss), str(avg_clf_loss)))
if FLAGS.f_save_step != None:
if current_step % FLAGS.f_save_step == FLAGS.f_save_step - 1:
logger.info('propotion:' + str(memory_pool.get_propotion()))
## save model ##
logger.info('Saving model...')
model_name = os.path.join(FLAGS.train_dir, 'imitator.model')
saver.save(sess, model_name)
logger.info('Save model sucess...')
def sample_thread(sess):
"""a thread used to collect the data from carla"""
begin = True
## set all the egopilots to autopilot and init whether_wait_red_light
whether_wait_red_light = {}
for egopilot in egopilots:
whether_wait_red_light[egopilot] = False
egopilot.set_autopilot(enabled=True)
while True:
if not random_drive:
logger.info('normal sample')
for camera, obj_collision, egopilot in zip(cameras, obj_collisions,
egopilots):
img = camera.get()
collision = obj_collision.get()
if collision:
obj_collision.clear()
## if collision skip this memory
single_execuate(target=respawn_actors,
args=(
world,
[egopilot],
))
## init the history of action
continue
cv2.imshow('test', img)
img = img[int(FLAGS.img_height * 1.8 //
5):, :, :] ## corp the ROI
cv2.imshow('test1', img)
img = img * 2. / 255. - 1.
img = cv2.resize(img, dsize=(418, 418))
cv2.imshow('test2', img)
std_steer = egopilot.get_control().steer
std_throttle = egopilot.get_control().throttle
std_brake = egopilot.get_control().brake
# print(std_steer)
scene_class, steer_scene_encode = steer_scene_classify(
steer=std_steer)
# if std_steer<0.05 and std_steer>-0.05:
# a = random.uniform(0,1)
# if a < 0.8:
# continue
ego_v = egopilot.get_velocity()
ego_v_ = math.sqrt(ego_v.x**2 + ego_v.y**2 + ego_v.z**2)
ego_v = ego_v_ / egopilot.get_speed_limit()
if egopilot.is_at_traffic_light(
) and not whether_wait_red_light[egopilot]:
## mean first go into traffic light area
if str(egopilot.get_traffic_light_state()) == 'Green':
# print('Green, straght forward')
scene_class = 9 ## mean straght forward
elif str(egopilot.get_traffic_light_state()) == 'Red':
# print('Red, Stop')
whether_wait_red_light[egopilot] = True
scene_class = 10 ## mean need to stop
light_state = light_state_encode[str(
egopilot.get_traffic_light_state())]
elif whether_wait_red_light[egopilot]:
if str(egopilot.get_traffic_light_state()) == 'Green':
whether_wait_red_light[egopilot] = False
# print('Red to Green, I can go now')
std_throttle = 1.
std_brake = 0.
# print('throttle:', std_throttle)
# print('brake:', std_brake)
scene_class = 11 ## mean red to green
elif str(egopilot.get_traffic_light_state()) == 'Red':
# print('Still Red, wait for green')
# whether_wait_red_light[egopilot] = True
scene_class = 10 ## mean need to stop
light_state = light_state_encode[str(
egopilot.get_traffic_light_state())]
else:
# if ego_v_ < 1e-3: ## imitate ahead obstcle
# steer_scene_encode = np.eye(1, 11, k=10)[0].astype(np.int32)
# # print("stop!!!")
# # else:
# # print('go forward')
light_state = light_state_encode['Unkown']
std_action = np.array([std_steer, std_throttle, std_brake])
other_state = np.concatenate(
[light_state, np.array([ego_v])], axis=-1)
# memory_pool.put(memory=[img.astype(np.float32), other_state.astype(np.float32), std_action.astype(np.float32),
# steer_scene_encode])
memory_pool.put(memory=[
img.astype(np.float32),
other_state.astype(np.float32),
std_action.astype(np.float32), steer_scene_encode
],
class_index=scene_class)
else:
logger.info('not sample')
for camera, obj_collision, egopilot in zip(cameras, obj_collisions,
egopilots):
img = camera.get()
collision = obj_collision.get()
if collision:
obj_collision.clear()
## if collision skip this memory
single_execuate(target=respawn_actors,
args=(
world,
[egopilot],
))
## init the history of action
continue
cv2.imshow('test', img)
# time.sleep(0.2)
cv2.waitKey(200)
#print(memory_pool.get_propotion())
if begin and memory_pool.is_balance():
begin = False
update_event.set()
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))
control = carla.VehicleControl(throttle=throttle)
other_vehicle.apply_control(control)
time.sleep(0.1)
if __name__ == '__main__':
##############################
####### general config #######
other_vehicle_init_pos = random.randint(40, 50)
other_vehicle_speed_range = (random.uniform(0.4, 0.5), random.uniform(0.6, 0.7))
##############################
#### carla world init ####
client = carla.Client('127.0.0.1', 2000)
client.set_timeout(10.0) # seconds
logger.info('Carla connect success...')
logger.info('Carla world initing...')
world = client.get_world()
destroy_all_actors(world)
## vehicle blueprint
blueprints = world.get_blueprint_library().filter('vehicle.nissan.micra')
blueprints = [x for x in blueprints if int(x.get_attribute('number_of_wheels')) == 4]
## host vehicle settings
host_vehicle_bp = random.choice(blueprints)
if host_vehicle_bp.has_attribute('color'):
color = random.choice(host_vehicle_bp.get_attribute('color').recommended_values)
host_vehicle_bp.set_attribute('color', color)
