def step(self, action): self.current_step += 1 self.action = action control = Control() control.throttle = 0.4 # action[0] control.steer = action[0] control.brake = 0 # action[2] try: self.client.send_control(control) observation, done = self._get_observation() except: print('Lost Connection') self.reset() self.client.send_control(control) observation, done = self._get_observation() reward = self._get_reward(done=done) info = {} #print("reward: ", reward) #print(observation, reward, done, info) observation = np.random.random((1, 512)) reward = np.random.random() done = False info = {} return observation, reward, done, info
def run_step(self, measurements, sensor_data, directions, target): rgb = sensor_data['CameraRGB'].data # publish data need to cut rgb = rgb[self._image_cut[0]:self._image_cut[1], :] rgb = scipy.misc.imresize(rgb, (224, 224)) rgb = np.expand_dims(preprocess_input(rgb), axis=0) if self.mode == 'DLM': intention = to_categorical([intention_mapping[directions]], num_classes=self.NUM_INTENTIONS) else: intention = np.expand_dims(preprocess_input(directions), axis=0) speed = np.array([measurements.player_measurements.forward_speed]) pred_control = self.model.predict([rgb, intention, speed]) control = Control() control.steer = pred_control[0][self.STEER] control.throttle = pred_control[0][self.GAS] if control.throttle < 0.0: control.brake = -control.throttle control.throttle = 0.0 control.hand_brake = 0 control.reverse = 0 return control, None
def _generate_test_case(self, poses_to_test): recording = Recording(name_to_save='TestMetric' , continue_experiment=False, save_images=True ) from carla.driving_benchmark.experiment import Experiment from carla.carla_server_pb2 import Measurements from carla.carla_server_pb2 import Control for pose in poses_to_test: experiment = Experiment() recording.write_summary_results(experiment=experiment, pose=pose, rep=1, path_distance=200, remaining_distance=0, final_time=0.2, time_out=49, result=1) reward_vec = [Measurements().player_measurements for x in range(25)] control_vec = [Control() for x in range(25)] recording.write_measurements_results(experiment=experiment, rep=1, pose=pose, reward_vec=reward_vec, control_vec=control_vec) return recording._path
def teste_write_measurements_results(self): import os from carla.driving_benchmark.experiment import Experiment from carla.carla_server_pb2 import Measurements from carla.carla_server_pb2 import Control recording = Recording(name_to_save='Test1', continue_experiment=False, save_images=True) reward_vec = [Measurements().player_measurements for x in range(20)] control_vec = [Control() for x in range(25)] recording.write_measurements_results(experiment=Experiment(), rep=1, pose=[24, 32], reward_vec=reward_vec, control_vec=control_vec) with open(os.path.join(recording._path, 'measurements.csv'), 'r') as f: header = f.readline().split(',') #Assert if header is header self.assertIn('exp_id', header) self.assertEqual(len(header), len(recording._dict_measurements)) #Assert if there is something writen in the row written_row = f.readline().split(',') #Assert if the number of collums is correct self.assertEqual(len(written_row), len(recording._dict_measurements))
def _compute_action(self, rgb_image, speed, direction=None): rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :] image_input = scipy.misc.imresize( rgb_image, [self._image_size[0], self._image_size[1]]) image_input = image_input.astype(np.float32) image_input = np.multiply(image_input, 1.0 / 255.0) steer, acc, brake = self._control_function(image_input, speed, direction, self._sess) # This a bit biased, but is to avoid fake breaking if brake < 0.1: brake = 0.0 if acc > brake: brake = 0.0 # We limit speed to 35 km/h to avoid if speed > 10.0 and brake == 0.0: acc = 0.0 control = Control() control.steer = steer control.throttle = acc control.brake = brake control.hand_brake = 0 control.reverse = 0 return control
def _compute_action(self,carla_direction, direction): start = time.time() # Predict the intermediate representations prediction = self._neural_net.predict(self._state.image_hist, [direction]) logging.info("Time for prediction: {}".format(time.time() - start)) logging.info("CARLA Direction {}, Real Direction {}".format(carla_direction, direction)) # update the speed limit if a speed limit sign is detected if prediction['speed_sign'][0] != -1: self._state.speed_limit = prediction['speed_sign'] # Calculate the control control = Control() control.throttle, control.brake = self._longitudinal_control(prediction, direction) control.steer = self._lateral_control(prediction) return control
def run_step(self, meas, sensory, directions, target): # print('Step {}'.format(self.step)) if self.step % self.frameskip == 0: obs_preprocessed = self.preprocess_data(meas, sensory, directions, target) action_idx = self.actor.act(obs_preprocessed=obs_preprocessed) action = self.actions[action_idx] control = Control() if self.obs_dict['speed'] < 30.: control.throttle = action[0] elif control.throttle > 0.: control.throttle = 0. control.steer = action[1] self.prev_control = control else: control = self.prev_control # print('Repeating control') self.step += 1 print(control.throttle, control.steer) return control
def _compute_action(self, rgb_image, speed, direction=None): rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :] image_input = scipy.misc.imresize( rgb_image, [self._image_size[0], self._image_size[1]]) if self.min_frames <= self.frame_count <= self.max_frames: scipy.misc.imsave( self.clean_frame_pt + str(self.frame_count) + '.png', image_input) input_info_dict = {} input_info_dict["forward_speed"] = speed input_info_dict["directions"] = direction input_info_file = self.input_pt + str(self.frame_count) + ".pickle" with open(input_info_file, "wb") as handle: pickle.dump(input_info_dict, handle, protocol=pickle.HIGHEST_PROTOCOL) image_input = image_input.astype(np.float32) image_input = np.multiply(image_input, 1.0 / 255.0) steer, acc, brake = self._control_function(image_input, speed, direction, self._sess) if brake < 0.1: brake = 0.0 if acc > brake: brake = 0.0 # We limit speed to 35 km/h to avoid if speed > 10.0 and brake == 0.0: acc = 0.0 steer_dict = {} steer_dict["steer"] = steer steer_dict_file = self.steer_deviation_clean_pt + str( self.frame_count) + ".pickle" with open(steer_dict_file, "wb") as handle: pickle.dump(steer_dict, handle, protocol=pickle.HIGHEST_PROTOCOL) control_info_file = self.control_info_pt + str( self.frame_count) + ".pickle" control = Control() with open(control_info_file, "rb") as handle: control_dict = pickle.load(handle) control.steer = control_dict["steer"] control.throttle = control_dict["throttle"] control.brake = control_dict["brake"] control.hand_brake = 0 control.reverse = 0 return control
def _compute_action(self, rgb_image, speed, direction=None): rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :] image_input = scipy.misc.imresize( rgb_image, [self._image_size[0], self._image_size[1]]) image_input = image_input.astype(np.float32) #为了节约空间之后调用浅拷贝变为tensor,所以这里要自己处理(H,W,C)->(N,C,H,W) image_input = np.expand_dims(np.transpose(image_input, (2, 0, 1)), axis=0) #标准化 image_input = np.multiply(image_input, 1.0 / 255.0) #这里速度表示与数据集里的不统一,这里的25就是数据集中的90km/h speed = np.array([[speed]]).astype(np.float32) / 25.0 direction = int(direction - 2) steer, acc, brake = self._control_function(image_input, speed, direction) #这里开始代码与官方示例完全一样 # This a bit biased, but is to avoid fake breaking if brake < 0.1: brake = 0.0 if acc > brake: brake = 0.0 if speed > 10.0 and brake == 0.0: acc = 0.0 if np.abs(steer) > 0.15: acc = acc * 0.4 control = Control() control.steer = steer control.throttle = acc control.brake = brake control.hand_brake = 0 control.reverse = 0 return control
def _compute_action(self, rgb_image, speed, direction=None): rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :] image_input = scipy.misc.imresize( rgb_image, [self._image_size[0], self._image_size[1]]) image_input = image_input.astype(np.float32) image_input = np.expand_dims(np.transpose(image_input, (2, 0, 1)), axis=0) image_input = np.multiply(image_input, 1.0 / 255.0) speed = np.array([[speed]]).astype(np.float32) / 25.0 direction = int(direction - 2) steer, acc, brake = self._control_function(image_input, speed, direction) # This a bit biased, but is to avoid fake breaking if brake < 0.1: brake = 0.0 if acc > brake: brake = 0.0 # We limit speed to 35 km/h to avoid if speed > 10.0 and brake == 0.0: acc = 0.0 if np.abs(steer) > 0.15: acc = acc * 0.4 control = Control() control.steer = steer control.throttle = acc control.brake = brake control.hand_brake = 0 control.reverse = 0 return control
def get_keyboard_control(self, keys): """ Return a VehicleControl message based on the pressed keys. Return None if a new episode was requested. """ if keys[K_r]: return None control = Control() if keys[K_LEFT] or keys[K_a]: control.steer = -1.0 if keys[K_RIGHT] or keys[K_d]: control.steer = 1.0 if keys[K_UP] or keys[K_w]: control.throttle = 1.0 if keys[K_DOWN] or keys[K_s]: control.brake = 1.0 if keys[K_SPACE]: control.hand_brake = True if keys[K_q]: self._is_on_reverse = not self._is_on_reverse if keys[K_p]: self._enable_manual_control = not self._enable_manual_control control.reverse = self._is_on_reverse return control
def run_step(self, measurements, sensor_data, directions, target): steer = self.cur_action['steer'] acc = self.cur_action['acc'] brake = self.cur_action['brake'] control = Control() feature_vector = self.DP.compute_feature(sensor_data) speed = measurements.player_measurements.forward_speed speed = speed / 10.0 offroad = measurements.player_measurements.intersection_offroad other_lane = measurements.player_measurements.intersection_otherlane state = np.concatenate( (feature_vector, (steer, acc - brake, speed, offroad, other_lane))) action = self.predict(np.expand_dims(state, 0))[0] control.steer = action[0] self.next_action['steer'] = action[0] if action[1] > 0: control.throttle = action[1] self.next_action['acc'] = action[1] control.brake = 0 self.next_action['break'] = 0 else: control.throttle = 0 self.next_action['acc'] = 0 control.brake = action[1] self.next_action['break'] = action[1] action_lambda = 0.5 control.steer = self.next_action['steer'] * action_lambda + ( 1 - action_lambda) * self.cur_action['steer'] control.throttle = self.next_action['acc'] * action_lambda + ( 1 - action_lambda) * self.cur_action['acc'] control.brake = self.next_action['break'] * action_lambda + ( 1 - action_lambda) * self.cur_action['brake'] control.hand_brake = 0 control.reverse = 0 self.cur_action['steer'] = control.steer self.cur_action['acc'] = control.throttle self.cur_action['brake'] = control.brake if measurements.player_measurements.forward_speed >= 8: control.throttle = 0.5 if control.throttle > 0.5 else control.throttle return control
def dim_pid_controller(self, dim_plan, plan_index, steer_controller, throttle_controller, brake_controller, transform_before_to_now, yaw_now, yaw_plan_start, current_obs): # control = carla_protocol.Control() control = VehicleControl() measurement = current_obs.measurements[-1] p = measurement.player_measurements current_forward_speed = p.forward_speed # Use offset of -1 to account for already incrementing plan_index plan_position_tform_start = dim_plan.get_plan_position( t_future_index=self.dimconf.position_setpoint_index + plan_index - 1) # Transform the waypoint in the old coordinates to the current. plan_position_tform_start_3d = np.concatenate( (plan_position_tform_start, [0])) position_setpoint = (transform_before_to_now.transform_points( plan_position_tform_start_3d[None]))[0] # Get the x coord of the position setpoint in the local car coords of current time. # How many meters at a specific point in the future we should be from where we are now. forward_setpoint = position_setpoint[0] # The plan positions are time-profiled. Hardcoded here to assume 5Hz. plan_Hz = 10 plan_period = 1. / plan_Hz T = 20 time_points = np.arange(plan_period, plan_period * T, plan_period) # How many seconds the target spatiotemporal forward point is in the future. time_offset = time_points[self.dimconf.position_setpoint_index] # The target forward speed along the orientation. target_forward_speed = forward_setpoint / time_offset # [TODO] Hacking this in for plotting purposes. dim_plan.current_target_forward_speed = target_forward_speed dim_plan.current_forward_speed_error = target_forward_speed - current_forward_speed dim_plan.current_target_forward_displacement = forward_setpoint dim_plan.plan_step = self.plan_index dim_plan.total_plan_count = self.total_plan_count # Use the headings derived by aiming towards points in the plan. # Use offset of -1 to account for already incrementing plan_index angle_position_tform_start = dim_plan.get_plan_position( t_future_index=min( self.dimconf.angle_setpoint_index + plan_index - 1, self.model.metadata.T - 1)) # Transform the waypoint in the old coordinates to the current. angle_position_tform_start_3d = np.concatenate( (angle_position_tform_start, [0])) # Get the current position to aim to in current car's coords. angle_setpoint_tform = (transform_before_to_now.transform_points( angle_position_tform_start_3d[None]))[0] # Force x coordinates in front. plan_angle_plancoords = np.arctan2(angle_setpoint_tform[1], abs(angle_setpoint_tform[0])) current_angle_plancoords = 0. # Steer controller is controlling to the target angle in the plan. steer_unsnapped = steer_controller.update( target_angle=plan_angle_plancoords, current_angle=current_angle_plancoords) # Update the PID controllers. throttle = throttle_controller.update( setpoint=target_forward_speed, process_variable=current_forward_speed) need_to_brake = (target_forward_speed - current_forward_speed) < 0. if need_to_brake: brake = -throttle throttle = 0.0 else: throttle = throttle brake = 0.0 # Clip the steering to the valid steering range steer = max(-1., min(1., steer_unsnapped)) brake = max(min(brake, 1.0), 0.0) throttle = min(max(throttle, 0.0), 1.0) # [TODO] hardcoded off. hand_brake = False reverse = False # Set the attributes of the control message. control.throttle = throttle control.brake = brake control.steer = steer control.hand_brake = hand_brake control.reverse = reverse return control
def run_step(self, measurements, sensor_data, directions, target): img = sensor_data['CameraRGB'].data filepath = os.path.join(DEFAULT_DATA_DIR, "imgs") cv2.imwrite(os.path.join(filepath, "img_" + str(self.step) + ".png"), img) rgb_image = img[self._image_cut[0]:self._image_cut[1], :] cv2.imwrite(os.path.join(filepath, "img_" + str(self.step) + "xx.png"), rgb_image) image_input = cv2.resize( rgb_image, (self._image_size[1], self._image_size[0]), interpolation=cv2.INTER_AREA) cv2.imwrite(os.path.join(filepath, "img_" + str(self.step) + "yy.png"), image_input) self.step += 1 # image_input = scipy.misc.imresize(rgb_image, [self.image_size[0], # self.image_size[1]]) image_input = image_input.astype(np.float32) image_input = np.multiply(image_input, 1.0 / 255.0) # speed = measurements.player_measurements.forward_speed speed = 0 image_input = np.expand_dims(image_input, 0) speed = np.expand_dims(speed, 0) speed = np.expand_dims(speed, 0) preds = self.model([image_input, speed], False) if directions == 0.0: pred = preds[0] else: directions -= 2 directions = int(directions) pred = preds[directions] steer = pred[0][0] acc = pred[0][1] brake = pred[0][2] if brake < 0.1: brake = 0.0 if acc > brake: brake = 0.0 # We limit speed to 35 km/h to avoid if speed > 10.0 and brake == 0.0: acc = 0.0 control = Control() control.steer = steer control.throttle = acc control.brake = brake control.hand_brake = 0 control.reverse = 0 print("steer:", control.steer, "throttle:", control.throttle, "brake:", control.brake) return control
def run_carla_client(args): # Here we will run 3 episodes with 300 frames each. numcams = 5 framestart = 20 # To compensate for the lag while starting an episode total_frames = args.total_frames datasize = total_frames / numcams # turn points in Town01 chosen = [94, 97, 99, 102, 42, 70, 85, 44, 46, 67, 69] # # turn points in Town01 for reversed # reverse = [95, 98, 100, 101, 103, 39, 49, 71, 84, 45, 47, 66, 68, 78] # chosen = chosen + reverse record_weathers = [5] number_of_episodes = len(chosen) frames_per_episode = datasize / number_of_episodes + framestart # chosen = [10] # number_of_episodes = 1 # frames_per_episode = 3000 # We assume the CARLA server is already waiting for a client to connect at # host:port. To create a connection we can use the `make_carla_client` # context manager, it creates a CARLA client object and starts the # connection. It will throw an exception if something goes wrong. The # context manager makes sure the connection is always cleaned up on exit. if args.humandriver: driver = HumanDriver() with make_carla_client(args.host, args.port) as client: print('CarlaClient connected') for weather in record_weathers: if args.record: folder_name = str(datetime.datetime.today().day) + '_' + 'Carla_' + '_' + args.experiment_name \ + 'Weather_' + str(weather) camera_dict, angles = get_camera_dict(args.settings_filepath, args.cameras) res = [int(i) for i in args.resolution.split(',')] image_cut = [int(i) for i in args.image_cut.split(',')] print(res, image_cut) recorder = Recorder(args.path_to_save + folder_name + '/', res,\ image_cut=image_cut,camera_dict=camera_dict, angles=angles) start = 0 for episode in range(0, number_of_episodes): # Start a new episode. if args.settings_filepath is None: # Create a CarlaSettings object. This object is a wrapper around # the CarlaSettings.ini file. Here we set the configuration we # want for the new episode. settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=0, NumberOfPedestrians=4, WeatherId=random.choice([1, 3, 7, 8, 14]), QualityLevel=args.quality_level) settings.randomize_seeds() else: # Alternatively, we can load these settings from a file. with open(args.settings_filepath, 'r') as fp: settings = fp.read() print(settings) settings = settings[:-2] + "\nWeatherId=" + str(weather) settings = settings + '\n\n[CARLA/QualitySettings]\nQualityLevel=' + args.quality_level # Now we load these settings into the server. The server replies # with a scene description containing the available start spots for # the player. Here we can provide a CarlaSettings object or a # CarlaSettings.ini file as string. scene = client.load_settings(settings) # Choose one player start at random. number_of_player_starts = len(scene.player_start_spots) player_start = chosen[ start] #random.randint(0, max(0, number_of_player_starts - 1)) print("Iter is ", start, "Start used is ", player_start) start += 1 start = start % len(chosen) # Notify the server that we want to start the episode at the # player_start index. This function blocks until the server is ready # to start the episode. print('Starting new episode...') client.start_episode(player_start) # Iterate every frame in the episode. ct = 0 recordFrame = 0 # for frame in range(0, frames_per_episode): while recordFrame < frames_per_episode: # Read the data produced by the server this frame. measurements, sensor_data = client.read_data() # for agent in measurements.non_player_agents: # if agent.HasField('traffic_light'): # agent.traffic_light.state = 0 # im = Image.frombytes(mode='RGBA', size=(200, 88), data=sensor_data['Camera0'].raw_data, decoder_name="raw") # im.save('test/test.png') # Save the images to disk if requested. if args.save_images_to_disk: for name, measurement in sensor_data.items(): filename = args.out_filename_format.format( episode, name, recordFrame) measurement.save_to_disk(filename) # We can access the encodnearested data of a given image as numpy # array using its "data" property. For instance, to get the # depth value (normalized) at pixel X, Y # # depth_array = sensor_data['CameraDepth'].data # value_at_pixel = depth_array[Y, X] # # Now we have to send the instructions to control the vehicle. # If we are in synchronous mode the server will pause the # simulation until we send this control. control = Control() if args.humandriver: control = driver.computeControl() client.send_control(control) elif args.autopilot: # Together with the measurements, the server has sent the # control that the in-game autopilot would do this frame. We # can enable autopilot by sending back this control to the # server. We can modify it if wanted, here for instance we # will add some noise to the steer. control = measurements.player_measurements.autopilot_control # control.steer += random.uniform(-0.1, 0.1) client.send_control(control) else: control.steer = random.uniform(-1.0, 1.0) control.throttle = 0.5 control.brake = 0.0 control.hand_brake = False control.reverse = False client.send_control(control) if args.record: if recordFrame > framestart: direction = 2 actions = control action_noisy, drifting_time, will_drift = recorder._noiser.compute_noise( actions, measurements.player_measurements.forward_speed) print("Noise diff", actions.steer - action_noisy.steer, actions.throttle - action_noisy.throttle, actions.brake - action_noisy.brake) if measurements.player_measurements.forward_speed <= 0.1: print("No move Frame no: ", recordFrame) else: print("Yes!! move Frame no: ", recordFrame) recorder.record(measurements, sensor_data, actions, action_noisy, direction) recordFrame += 1 else: recordFrame += 1
def run_step(self, measurements, sensor_data, directions, target): img = sensor_data['CameraRGB'].data res = self.process(img) res[1] = np.expand_dims(res[1], 0) pred = self.predict(res) steer, acc, brake = pred[0], pred[1], pred[2] control = Control() control.steer = steer if acc < 0.3: control.throttle = 0 elif acc < 0.7: control.throttle = 0.5 else: control.throttle = 1 speed = measurements.player_measurements.forward_speed if speed * 3.6 >= 20: control.throttle = 0 if brake < 0.3: control.brake = 0 elif brake < 0.7: control.brake = 0.5 else: control.brake = 1 control.hand_brake = 0 control.reverse = 0 return control