def run_step(self, measurements, sensor_data, directions, target):
#control_agent = self._agent.run_step(measurements, None, target)
print(" RUnning STEP ")
speed = torch.cuda.FloatTensor(
[measurements.player_measurements.forward_speed]).unsqueeze(0)
print("Speed is", speed)
print("Speed shape ", speed)
directions_tensor = torch.cuda.LongTensor([directions])
# model_outputs = self.model.forward_branch(self._process_sensors(sensor_data), speed,
# directions_tensor)
if self.architecture_name == 'wgangp_lsd':
embed, model_outputs = self.model(
self._process_sensors(sensor_data), speed)
elif self.architecture_name == 'coil_unit':
embed, n_b = self.model_gen.encode(
self._process_sensors(sensor_data))
model_outputs = self.model_task(embed, speed)
elif self.architecture_name == 'unit_task_only':
embed, n_b = self.model_gen.encode(
self._process_sensors(sensor_data))
model_outputs = self.model_task(embed, speed)
elif self.architecture_name == 'coil_icra':
model_outputs = self.model.forward_branch(
self._process_sensors(sensor_data), speed, directions_tensor)
print(model_outputs)
if self.architecture_name == 'coil_icra':
steer, throttle, brake = self._process_model_outputs(
model_outputs[0],
measurements.player_measurements.forward_speed)
else:
steer, throttle, brake = self._process_model_outputs(
model_outputs[0][0],
measurements.player_measurements.forward_speed)
control = carla_protocol.Control()
control.steer = steer
control.throttle = throttle
control.brake = brake
# if self._auto_pilot:
# control.steer = control_agent.steer
# TODO: adapt the client side agent for the new version. ( PROBLEM )
#control.throttle = control_agent.throttle
#control.brake = control_agent.brake
# TODO: maybe change to a more meaningfull message ??
return control
def replay_control(self):
if self.t == 0:
self.chosen_control_trajectory = self.all_controls[
np.random.choice(len(self.all_controls))]
log.info("Chose new control trajectory")
control = carla_protocol.Control()
control.hand_brake = False
control.reverse = False
control.steer = self.chosen_control_trajectory[self.t][0]
control.throttle = self.chosen_control_trajectory[self.t][1]
self.t += 1
return control
def pilot_with_PID_steer_controller(waypoints_local_2d, coeff_steer):
angle = np.arctan2(waypoints_local_2d[0, 1], waypoints_local_2d[0, 0])
steer = min(max(coeff_steer * angle, -1.), 1.)
throttle = 0.5
brake = 0. # not considered yet
control = carla_protocol.Control()
control.steer = steer
control.throttle = throttle
control.brake = brake
control.hand_brake = False
control.reverse = False
return control
def noisy_autopilot(measurement, replan_index, replan_period, cfg):
# 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.
autocontrol = measurement.player_measurements.autopilot_control
control = carla_protocol.Control()
brake = autocontrol.brake
hand_brake = autocontrol.hand_brake
steer = autocontrol.steer
throttle = autocontrol.throttle
reverse = autocontrol.reverse
# Autopilot perturbation.
if cfg.add_steering_noise:
# Sometimes add noise.
if random.random() > 0.5:
steer_noise = 0.1
snoise = random.uniform(-steer_noise, steer_noise)
steer += snoise
log.debug("snoise {}. steer".format(snoise, steer))
# Clip before CARLA internally clips them.
steer = max(min(steer, 1.), -1.)
if cfg.add_throttle_noise:
if random.random() > 0.5:
if brake > 0.2:
throttle_noise = 0.0
else:
throttle_noise = 0.1
brake = 0.0
tnoise = random.uniform(-throttle_noise, throttle_noise)
throttle += tnoise
log.debug("tnoise {}. throttle: {}".format(tnoise, throttle))
throttle = max(min(throttle, 1.0), 0.0)
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):
self.model.eval()
#control_agent = self._agent.run_step(measurements, None, target)
print(" RUnning STEP ")
speed = torch.cuda.FloatTensor(
[measurements.player_measurements.forward_speed]).unsqueeze(0)
print("Speed is", speed)
print("Speed shape ", speed)
directions_tensor = torch.cuda.LongTensor([directions])
embed, model_outputs = self.model(self._process_sensors(sensor_data),
speed)
# model_outputs = self.model.forward_branch(self._process_sensors(sensor_data), speed,
# directions_tensor)
print(model_outputs)
steer, throttle, brake = self._process_model_outputs(
model_outputs[0][0],
measurements.player_measurements.forward_speed)
#control = self.compute_action(,
# ,
# directions)
control = carla_protocol.Control()
control.steer = steer
control.throttle = throttle
control.brake = brake
# if self._auto_pilot:
# control.steer = control_agent.steer
# TODO: adapt the client side agent for the new version. ( PROBLEM )
#control.throttle = control_agent.throttle
#control.brake = control_agent.brake
# TODO: maybe change to a more meaningfull message ??
return control
def run_step(self, measurements, sensor_data, directions, target):
# pos = (rewards.player_x,rewards.player_y,22)
# ori =(rewards.ori_x,rewards.ori_y,rewards.ori_z)
# pos,point = self.planner.get_defined_point(pos,ori,(target[0],target[1],22),(1.0,0.02,-0.001),self._select_goal)
# direction = convert_to_car_coord(point[0],point[1],pos[0],pos[1],ori[0],ori[1])
# image_filename_format = '_images/episode_{:0>3d}/{:s}/image_{:0>5d}.png'
# sys.stdout = open(str("direction" + ".out", "a", buffering=1))
#control_agent = self._agent.run_step(measurements, None, target)
print(" RUnning STEP ")
speed = torch.cuda.FloatTensor(
[measurements.player_measurements.forward_speed]).unsqueeze(0)
print("Speed shape ", speed)
directions_tensor = torch.cuda.LongTensor([2])
print("dir", directions_tensor)
model_outputs = self.model.forward_branch(
self._process_sensors(sensor_data), speed, directions_tensor)
print(model_outputs)
steer, throttle, brake = self._process_model_outputs(
model_outputs[0], measurements.player_measurements.forward_speed)
#control = self.compute_action(,
# ,
# directions)
control = carla_protocol.Control()
control.steer = steer
control.throttle = throttle
control.brake = brake
# if self._auto_pilot:
# control.steer = control_agent.steer
# TODO: adapt the client side agent for the new version. ( PROBLEM )
#control.throttle = control_agent.throttle
#control.brake = control_agent.brake
# TODO: maybe change to a more meaningfull message ??
return control
def update(self, measurement, current_angle, target_angle,
position_setpoint):
forward_speed = measurement.player_measurements.forward_speed
# Steer controller is controlling to the target angle in the plan.
steer_unsnapped = self.steer_controller.update(
target_angle=target_angle, current_angle=current_angle)
heading_2d = [1., 0.]
# Compute distance of the setpoint along the heading.
horizon_meters = compute_setpoint_signed_distance(
setpoint=position_setpoint, heading_2d=heading_2d)
# Target seconds to match
target_forward_speed = horizon_meters / self.horizon_seconds
# Update the PID controllers.
# Throttle controller is controlling to a position in local coordinates.
throttle_unsnapped = self.throttle_controller.update(
setpoint=target_forward_speed, process_variable=forward_speed)
log.debug(
"\n\tSigned distance {:.3f}".format(horizon_meters) +
"\n\tForward speed {:.3f}".format(forward_speed) +
"\n\tTarget forward speed {:.3f}".format(target_forward_speed) +
"\n\tThrottle unsnapped: {:.3f}".format(throttle_unsnapped))
control = carla_protocol.Control()
control.steer = clip_steer(steer_unsnapped)
control.throttle = clip_throttle(throttle_unsnapped)
# if self.brake_controller is not None:
# brake = self.brake_controller.update(
# control.brake = max(min(brake, 1.0), 0.0)
control.brake = 0.0
control.hand_brake = False
control.reverse = False
if throttle_unsnapped < -5:
log.warning("Hacking very negative throttle to braking")
control.brake = 1.0
return control
def load_control_at_frame(directory, frame):
filename = op.join(directory, 'Control_{:06d}.pb'.format(frame))
with open(filename, 'rb') as f:
control = carla_protocol.Control()
control.ParseFromString(f.read())
return control