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)