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
