def _get_joystick_control(self,joystick):
control = VehicleControl()
tmp1 = 0.6 * joystick.get_axis(1)
if (tmp1 <= 0):
control.throttle = -tmp1
control.brake = 0
else:
control.throttle = 0
control.brake = tmp1
control.steer = joystick.get_axis(2)
control.steer = 0.5 * control.steer * control.steer * control.steer
# print('steer....',control.steer)
#provide a stable autopilot
autopilot = joystick.get_button(0)
if autopilot == 1:
self._enable_autopilot = not self._enable_autopilot
# provide a stable reverse
reverse = joystick.get_button(2)
if reverse == 1:
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
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 = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
control.steer = -0.5
if keys[K_RIGHT] or keys[K_d]:
control.steer = 0.5
if keys[K_UP] or keys[K_w]:
control.throttle = 0.7
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_autopilot = not self._enable_autopilot
if keys[K_2]:
self._command = 2
if keys[K_3]:
self._command = 3
if keys[K_4]:
self._command = 4
if keys[K_5]:
self._command = 5
control.reverse = self._is_on_reverse
return control
def send_control_command(client,
throttle,
steer,
brake,
hand_brake=False,
reverse=False):
"""Send control command to CARLA client.
Send control command to CARLA client.
Args:
client: The CARLA client object
throttle: Throttle command for the sim car [0, 1]
steer: Steer command for the sim car [-1, 1]
brake: Brake command for the sim car [0, 1]
hand_brake: Whether the hand brake is engaged
reverse: Whether the sim car is in the reverse gear
"""
control = VehicleControl()
# Clamp all values within their limits
steer = np.fmax(np.fmin(steer, 1.0), -1.0)
throttle = np.fmax(np.fmin(throttle, 1.0), 0)
brake = np.fmax(np.fmin(brake, 1.0), 0)
control.steer = steer
control.throttle = throttle
control.brake = brake
control.hand_brake = hand_brake
control.reverse = reverse
client.send_control(control)
def control(self):
vcontrol = VehicleControl()
# Press Y
if self.controller.get_button(3):
return None
# Left stick
steer = self.controller.get_axis(0)
if abs(steer) >= 0.1:
vcontrol.steer = steer / 2.
# Right stick
throttle = -self.controller.get_axis(4)
if throttle > 0:
vcontrol.throttle = abs(throttle) / 1.5
else:
vcontrol.brake = abs(throttle)
# Press right stick
if self.controller.get_button(10):
vcontrol.hand_brake = True
self.client.send_control(vcontrol)
self.info[
'Throttle'] = vcontrol.throttle if throttle > 0 else -vcontrol.brake
self.info['Steer'] = vcontrol.steer
def get_control_from_a(a):
control = VehicleControl()
control.steer = a[0]
control.throttle = a[1]
control.brake = a[2]
control.hand_brake = bool(a[3])
control.reverse = bool(a[4])
return control
def perform_action(self, data):
'''
Performs action
Input : Data = [image, segmentation, depth, ...]
Return : control
action = [throttle, steering_angle, steering_direction, brake, reverse_gear]
'''
action = self.agent.select_action(data, self.memory)
print("obtained action")
#########################################
# TODO
# Add randomness in run_step of agent
#########################################
control = VehicleControl()
# Car Throttle
control.throttle = action[0]
################################################################
# TODO
# We need to convert angle into the units which carla uses
# steering direction will be a bool 0 for left and 1 for right
if action[2]:
control.steer = function(action[1])
else:
control.steer = -1 * function(action[1])
################################################################
# brake is a bool
control.brake = action[3]
# If needed there is an option for hand_brake as well
# control.hand_brake
# Car reverse gear
if action[4]:
self.carla_game._is_on_reverse = not self.carla_game._is_on_reverse
control.reverse = self.carla_game._is_on_reverse
print("returning control..")
return control, action
def _get_XBOX_control(self, joystick):
control = VehicleControl()
control.throttle = 0.4 * (joystick.get_axis(5) + 1)
if control.throttle > 0.6:
control.throttle = 0.6
# if control.throttle <0.3:
# control.throttle = 0
# elif control.throttle<0.7:
# control.throttle = 0.5
# else:
# control.throttle =1.0
control.brake = 0.5 * (joystick.get_axis(2) + 1)
control.brake = max(0, control.brake - 0.1)
control.steer = joystick.get_axis(0)
if(abs(control.steer)<0.05):
control.steer = 0
if control.steer <=-0.05:
control.steer += 0.05
if control.steer >=0.05:
control.steer -= 0.05
control.steer = 0.8 * control.steer
control.reverse = self._is_on_reverse
# command = joystick.get_hat(0)
# if command[0] == -1:
# self._command = 3
# elif command[0] == 1:
# self._command = 4
# if command[1] == -1:
# self._command = 5
# elif command[1] == 1:
# self._command = 2
# return control
if joystick.get_axis(3) > 0.5:
self._command = 4
elif joystick.get_axis(3) < -0.5:
self._command = 3
elif joystick.get_axis(4) > 0.5:
self._command = 5
elif joystick.get_axis(4) < -0.5:
self._command = 2
return control
def _get_autopilot_control(self, measurements):
control = VehicleControl()
control.steer = measurements.player_measurements.autopilot_control.steer
control.throttle = measurements.player_measurements.autopilot_control.throttle
control.brake = measurements.player_measurements.autopilot_control.brake
control.hand_brake = measurements.player_measurements.autopilot_control.hand_brake
control.reverse = measurements.player_measurements.autopilot_control.reverse
return control
def _get_XBOX_control(self, joystick):
control = VehicleControl()
control.throttle = 0.4 * (joystick.get_axis(5) + 1)
if control.throttle > 0.6:
control.throttle = 0.6
# if control.throttle <0.3:
# control.throttle = 0
# elif control.throttle<0.7:
# control.throttle = 0.5
# else:
# control.throttle =1.0
control.brake = 0.5 * (joystick.get_axis(2) + 1)
control.brake = max(0, control.brake - 0.1)
control.steer = joystick.get_axis(0)
if (abs(control.steer) < 0.05):
control.steer = 0
if control.steer <= -0.05:
control.steer += 0.05
if control.steer >= 0.05:
control.steer -= 0.05
control.steer = 0.8 * control.steer
control.reverse = self._is_on_reverse
# command = joystick.get_hat(0)
# if command[0] == -1:
# self._command = 3
# elif command[0] == 1:
# self._command = 4
# if command[1] == -1:
# self._command = 5
# elif command[1] == 1:
# self._command = 2
# return control
if joystick.get_axis(3) > 0.5:
self._command = 4
elif joystick.get_axis(3) < -0.5:
self._command = 3
elif joystick.get_axis(4) > 0.5:
self._command = 5
elif joystick.get_axis(4) < -0.5:
self._command = 2
return control
def get_control(self, wp_angle, wp_angle_speed, speed_factor, current_speed ,steer ,acc,brake) :
# NOTE! All the calculations inside this function are made assuming speed in Km/h
control = VehicleControl()
current_speed = max(current_speed, 0)
if steer > 0:
control.steer = min(steer, 1)
else:
control.steer = max(steer, -1)
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self.params['target_speed'] * speed_factor
# Depending on the angle of the curve the speed is either 20 (beginning) 15 (most of it)
elif math.fabs(wp_angle_speed) < 0.5:
target_speed_adjusted = 20 * speed_factor
else:
target_speed_adjusted = 15 * speed_factor
self.pid.target = target_speed_adjusted
pid_gain = self.pid(feedback=current_speed)
#print ('Target: ', self.pid.target, 'Error: ', self.pid.error, 'Gain: ', pid_gain)
#print ('Target Speed: ', target_speed_adjusted, 'Current Speed: ',
# current_speed, 'Speed Factor: ', speed_factor)
throttle = min( acc , max(self.params['default_throttle'] - 1.3 * pid_gain, 0),
self.params['throttle_max'])
if pid_gain > 0.5:
brake = max(brake , min(0.35 * pid_gain * self.params['brake_strength'], 1))
else:
brake = 0
if throttle > brake:
brake = 0
else:
throttle = 0
control.throttle = throttle#, acc)
control.brake = brake
# print ('Throttle: ', control.throttle, 'Brake: ',
# control.brake, 'Steering Angle: ', control.steer)
return control
def _on_loop(self, frame):
self._timer.tick()
skip_frames = 40
measurements, sensor_data = self.client.read_data()
current_position = vec3tovec2(measurements.player_measurements.transform.location)
self._velocities.append(measurements.player_measurements.forward_speed * 3.6) # convert to km/h
steer = 0.0
acceleration = 0.0
for name, measurement in sensor_data.items():
model_input = preprocess(measurement.data)
model_input = np.array(model_input / 127.5 - 1, dtype=np.float32)
model_input = np.expand_dims(model_input, axis=0)
ret = self._model.predict(model_input)[0]
steer = ret[0]
acceleration = ret[1]
if USE_SPEED_CONSTRAINTS:
if measurements.player_measurements.forward_speed * 3.6 < MINIMAL_SPEED:
acceleration = 0.7
elif measurements.player_measurements.forward_speed * 3.6 > MAXIMAL_SPEED:
acceleration = 0
control = VehicleControl()
control.steer = steer
control.throttle = acceleration
self.client.send_control(control)
if frame < skip_frames:
logging.info("Skipping first {} frames...".format(skip_frames))
return True
self.line_points.append(current_position)
self.points_x.append(current_position[0])
self.points_y.append(current_position[1])
if len(self.line_points) > 1:
dist_from_start = distance(self.line_points[0], current_position)
else:
dist_from_start = 10000
if dist_from_start < 1 and frame > skip_frames + 100:
logging.info("Position: {} is already logged".format(current_position))
return False
if self._timer.elapsed_seconds_since_lap() > 0.5:
self._print_player_measurements(control)
logging.info("Add point: [{:.4f},{:.4f}], points count: {:0>4d}, distance from start: {:.4f}".format(
current_position[0],
current_position[1],
len(self.line_points),
dist_from_start))
self._timer.lap()
return True
def decode_control(self, cod):
#将数字的ｃｏｎｔｒｏｌ转换为VehicleControl()
control = VehicleControl()
control.steer = 0
control.throttle = 0
control.brake = 0
control.hand_brake = False
control.reverse = False
th, steer = cod
if th > 0:
control.throttle = min(th, 1.0)
if th < 0:
control.brake = min(-th, 1.0)
control.steer = max(min(steer, 1.0), -1.0)
# if cod > 9:
# control.hand_brake = True
# if cod == 10:
# control.steer = -1
# elif cod == 11:
# control.steer = 1
# return control
# if cod == 9:
# control.reverse = True
# control.throttle = 1
# return control
# if cod % 3 == 1:
# control.brake = 1
# elif cod % 3 == 2:
# control.throttle = 1
# if cod // 3 == 1:
# control.steer = 1
# elif cod // 3 == 2:
# control.steer = -1
return control
def get_autopilot_control(self, wp_angle, wp_angle_speed, speed_factor,
current_speed, measurements):
control = VehicleControl()
current_speed = max(current_speed, 0)
steer = measurements.player_measurements.autopilot_control.steer
if steer > 0:
control.steer = min(steer, 1)
else:
control.steer = max(steer, -1)
# Don't go to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self.params['target_speed'] * speed_factor
# Depending on the angle of the curve the speed is either 20 (beginning) 15 (most of it)
elif math.fabs(wp_angle_speed) < 0.5:
target_speed_adjusted = 20 * speed_factor
else:
target_speed_adjusted = 15 * speed_factor
self.pid.target = target_speed_adjusted
pid_gain = self.pid(feedback=current_speed)
# print ('Target: ', self.pid.target, 'Error: ', self.pid.error, 'Gain: ', pid_gain)
# print ('Target Speed: ', target_speed_adjusted, 'Current Speed: ',
# current_speed, 'Speed Factor: ', speed_factor)
throttle = min(
max(self.params['default_throttle'] - 1.3 * pid_gain, 0),
self.params['throttle_max'])
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self.params['brake_strength'], 1)
else:
brake = 0
control.throttle = max(throttle, 0)
control.brake = brake
# print ('Throttle: ', control.throttle, 'Brake: ',
# control.brake, 'Steering Angle: ', control.steer)
return control
def get_keyboard_control(keys, is_on_reverse, enable_autopilot):
if keys[K_r]:
return None
control = VehicleControl()
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]:
is_on_reverse = not is_on_reverse
if keys[K_p]:
enable_autopilot = not enable_autopilot
control.reverse = is_on_reverse
return control, is_on_reverse, enable_autopilot
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 = VehicleControl()
self.resetVal()
################################################### YOU CAN EDIT IT ACCORDING YOU....
if keys[K_LEFT] or keys[K_a]:
self._val1 = -1
print("Left")
if (pid.prev > 0):
pid.prev = 0
elif keys[K_RIGHT] or keys[K_d]:
self._val1 = 1
print("Right")
if (pid.prev < 0):
pid.prev = 0
pid.prev += self._val1
output = pid.kp * self._val1 + pid.ki * pid.prev
print(output)
control.steer = output #Imp Line
if keys[K_UP] or keys[K_w]:
self._val2 = 1
pid.prev = 0
elif keys[K_DOWN] or keys[K_s]:
control.brake = 1
pid.prev = 0
###
if (self._velocity < 77):
control.throttle = self._val2 #Imp Line
else:
control.throttle = self._val2 * 0.8
if keys[K_SPACE]:
control.hand_brake = True
pid.prev = 0
if keys[K_f]:
self._val3 = 1 - self._val3
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
pid.prev = 0
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
################################################################################
#print(control)
return control
def _get_joystick_control(self):
# define ids of important axes on joystick
TRIGGERS = 2
X_AXIS = 0
control = VehicleControl()
control.throttle = 0
control.brake = 0
# dont know why but axes in PYGAME are inverted, LEFT_TRIGGGER is >0 and RIGHT_TRIGGER <0
# LEFT_TRIGGER is for braking
trigger_value = self._current_joystick.get_axis(TRIGGERS)
if trigger_value > 0:
control.brake = trigger_value
# RIGHT_TRIGGER is for throttle
elif trigger_value < 0:
control.throttle = -trigger_value
control.steer = self._current_joystick.get_axis(X_AXIS)
if X_AXIS_DEADZONE[0] < control.steer < X_AXIS_DEADZONE[1]:
control.steer = 0
return control
def get_keyboard_control(self, char):
control = VehicleControl()
if char == 'a':
print 'got a'
control.steer = -1.0
if char == 'd':
print 'got d'
control.steer = 1.0
if char == 'w':
print 'got w'
control.throttle = 1.0
if char == 's':
print 'got s'
control.brake = 1.0
if char == ' ':
print 'got space'
control.hand_brake = True
if char == 'q':
print 'got q'
self._is_on_reverse = not self._is_on_reverse
control.reverse = self._is_on_reverse
return control
def action_to_control(self, action, last_measurements=None):
control = VehicleControl()
if self.action_type == 'carla-original':
if type(action) == np.ndarray:
action = action.item()
if type(action) != int:
print('Unexpected action got {}'.format(type(action)))
assert type(action) == int, 'Action should be an int'
action = self.discrete_actions[action]
if last_measurements is not None and last_measurements.player_measurements.forward_speed * 3.6 < 30:
control.throttle = action[0]
elif action[0] > 0.:
control.throttle = 0.
control.steer = action[1]
elif self.action_type == 'continuous':
control.throttle = min(1, max(0.0, action[0]))
control.brake = min(1, max(0.0, action[1]))
control.steer = min(1, max(-1, action[2]))
# print('Control: {}, {}, {}'.format(control.throttle, control.brake, control.steer))
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 = VehicleControl()
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
control.reverse = self._is_on_reverse
return control
def get_control(self, wp_angle, wp_angle_speed, speed_factor, current_speed):
control = VehicleControl()
current_speed = max(current_speed, 0)
steer = self.params['steer_gain'] * wp_angle
if steer > 0:
control.steer = min(steer, 1)
else:
control.steer = max(steer, -1)
# Don't go0 to fast around corners
if math.fabs(wp_angle_speed) < 0.1:
target_speed_adjusted = self.params['target_speed'] * speed_factor
elif math.fabs(wp_angle_speed) < 0.5:
target_speed_adjusted = 20 * speed_factor
else:
target_speed_adjusted = 15 * speed_factor
self.pid.target = target_speed_adjusted
pid_gain = self.pid(feedback=current_speed)
print ('Target: ', self.pid.target, 'Error: ', self.pid.error, 'Gain: ', pid_gain)
print ('Target Speed: ', target_speed_adjusted, 'Current Speed: ', current_speed, 'Speed Factor: ',
speed_factor)
throttle = min(max(self.params['default_throttle'] - 1.3 * pid_gain, 0),
self.params['throttle_max'])
if pid_gain > 0.5:
brake = min(0.35 * pid_gain * self.params['brake_strength'], 1)
else:
brake = 0
control.throttle = max(throttle, 0) # Prevent N by putting at least 0.01
control.brake = brake
print ('Throttle: ', control.throttle, 'Brake: ', control.brake, 'Steering Angle: ', control.steer)
return control
def _get_gamepad_control(self):
control = VehicleControl()
gamepad = GamepadManager()
control.steer = gamepad.axis_states[GamepadManager.AxisMap.LEFT_X]
control.throttle = (gamepad.axis_states[GamepadManager.AxisMap.RT] +
1) / 2.0
control.brake = (gamepad.axis_states[GamepadManager.AxisMap.LT] +
1) / 2.0
if gamepad.button_states[GamepadManager.ButtonMap.X]:
self._is_on_reverse = not self._is_on_reverse
if gamepad.button_states[GamepadManager.ButtonMap.B]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
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 = VehicleControl()
if keys[K_LEFT] or keys[K_a]:
#control.steer = -1.0
if self.steer > -1 and self.steer < 0:
control.steer = self.steer - .1
else:
control.steer = -1
self.steer = control.steer
if keys[K_RIGHT] or keys[K_d]:
#control.steer = 1.0
if self.steer > 0 and self.steer < 1:
control.steer = self.steer + .1
else:
control.steer = 1
self.steer = control.steer
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_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
return control
def _get_steering_control(self):
numAxes = self.js.get_numaxes()
jsInputs = [float(self.js.get_axis(i)) for i in range(numAxes)]
# print('Js inputs [%s]' % ', '.join(map(str, jsInputs)))
control = VehicleControl()
control.steer = jsInputs[0]
if ((1 - jsInputs[1]) / 2) > 0.001:
control.brake = (1 - jsInputs[1]) / 2
else:
control.brake = 0
if ((1 - jsInputs[2]) / 2) > 0.001:
control.throttle = (1 - jsInputs[2]) / 2
else:
control.throttle = 0
control.hand_brake = 0.0
control.reverse = 0.0
# print(control)
return control
def _get_autopilot_control(self):
speed = int(self._measurements.player_measurements.forward_speed * 3.6)
autopilot = self._measurements.player_measurements.autopilot_control
control = VehicleControl()
steer_noise = self._settings["autopilot_steer_noise"]
if steer_noise != 0:
steer_noise = np.random.uniform(-steer_noise, steer_noise)
control.steer = autopilot.steer + steer_noise
throttle_noise = (
self._settings["autopilot_throttle_noise"] if speed > 10 else 0
)
if throttle_noise != 0:
throttle_noise = np.random.uniform(-throttle_noise, throttle_noise)
control.throttle = autopilot.throttle + throttle_noise
control.brake = autopilot.brake
return control
def action_steering_wheel(self, jsInputs, jsButtons):
control = VehicleControl()
# Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
# For the steering, it seems fine as it is
K1 = 1.0 # 0.55
steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx])
K2 = 1.6 # 1.6
throttleCmd = K2 + (
2.05 * math.log10(-0.7 * jsInputs[self._throttle_idx] + 1.4) -
1.2) / 0.92
if throttleCmd <= 0:
throttleCmd = 0
elif throttleCmd > 1:
throttleCmd = 1
brakeCmd = 1.6 + (2.05 * math.log10(-0.7 * jsInputs[self._brake_idx] +
1.4) - 1.2) / 0.92
if brakeCmd <= 0:
brakeCmd = 0
elif brakeCmd > 1:
brakeCmd = 1
#print("Steer Cmd, ", steerCmd, "Brake Cmd", brakeCmd, "ThrottleCmd", throttleCmd)
control.steer = steerCmd
control.brake = brakeCmd
control.throttle = throttleCmd
toggle = jsButtons[self._reverse_idx]
if toggle == 1:
self._is_on_reverse += 1
if self._is_on_reverse % 2 == 0:
control.reverse = False
if self._is_on_reverse > 1:
self._is_on_reverse = True
if self._is_on_reverse:
control.reverse = True
control.hand_brake = False # jsButtons[self.handbrake_idx]
return control
def _on_loop(self):
self._timer.tick()
measurements, _ = self.client.read_data()
self.pubState(measurements.player_measurements)
# Print measurements every second.
if self._timer.elapsed_seconds_since_lap() > 1.0:
self._print_player_measurements(measurements.player_measurements)
# Plot position on the map as well.
self._timer.lap()
if self._vehicle_control.restart:
self._on_new_episode()
else:
control = VehicleControl()
control.throttle = self._vehicle_control.accel_cmd.accel
control.brake = self._vehicle_control.brake_cmd.brake
control.steer = self._vehicle_control.steer_cmd.steer
control.hand_brake = self._vehicle_control.hand_brake
control.reverse = self._vehicle_control.reverse
self.client.send_control(control)
def _on_loop(self):
self._timer.tick()
measurements, sensor_data = self.client.read_data()
control = VehicleControl()
self._main_image = sensor_data.get('CameraRGB', None)
#self._mini_view_image1 = sensor_data.get('CameraDepth', None)
try:
print("model starting")
print(self._main_image)
img = image_converter.to_rgb_array(self._main_image)
x = [img]
x = np.asarray(x)
print(x.shape)
control.steer, control.throttle = model.predict(x)[0]
print(control.steer, control.throttle)
except:
print("saala fir ruk gya")
## Print measurements every second.
if self._timer.elapsed_seconds_since_lap() > 1.0:
if self._city_name is not None:
# Function to get car position on map.
map_position = self._map.convert_to_pixel([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
# Function to get orientation of the road car is in.
lane_orientation = self._map.get_lane_orientation([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
self._print_player_measurements_map(
measurements.player_measurements, map_position,
lane_orientation)
else:
self._print_player_measurements(
measurements.player_measurements)
# Plot position on the map as well.
self._timer.lap()
##get keyboard commands->
# control = self._get_keyboard_control(pygame.key.get_pressed()) ############## IMP (used get_keyboard control func)
# Set the player position
if self._city_name is not None:
self._position = self._map.convert_to_pixel([
measurements.player_measurements.transform.location.x,
measurements.player_measurements.transform.location.y,
measurements.player_measurements.transform.location.z
])
self._agent_positions = measurements.non_player_agents
if control is None:
print("kuch nhi")
elif self._enable_autopilot:
self.client.send_control(
measurements.player_measurements.autopilot_control)
else:
self.client.send_control(control)
def steer_left(self): control = VehicleControl() control.steer = max(control.steer - 0.01, -0.05) control.reverse = self._is_on_reverse control.throttle = self.measurements.player_measurements.autopilot_control.throttle self.client.send_control(control)
def _get_keyboard_control(self, keys):
"""
Return a VehicleControl message based on the pressed keys. Return None
if a new episode was requested.
"""
#returns NONE to close simulator
if keys[K_r]:
return None
#control is set to its accurate type
control = VehicleControl()
#input set to manual
if self._input_control == "Manual":
if self._xbox_cont:
#updates and checks for input from controller
pygame.event.pump()
#get_axis values may differ depending on controller
#values weighted to be used on carla
control.steer = self._joystick.get_axis(3)
print(control.steer)
control.throttle = (self._joystick.get_axis(5) + 1) / 2
control.brake = (self._joystick.get_axis(2) + 1) / 2
self._Manual_steer = control.steer
else:
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
#replay of previously recorded data
elif self._input_control == "Replay":
#read replay.csv file, _replay_frame values gives us correct row
control.steer = replay.iloc[self._replay_frame, 4]
control.throttle = replay.iloc[self._replay_frame, 6]
#input set to autonomous, steer values from model
else:
control.steer = self._AI_steer
control.throttle = 0.5
print(control.steer)
if keys[K_l]:
self._data_collection = not self._data_collection
if self._data_collection:
print("Data Collection ON")
else:
print("Data Collection OFF")
time.sleep(0.05)
#Check for keyboard commands to change input control device
if keys[K_m]:
print("Manual Control")
self._input_control = "Manual"
if keys[K_k]:
print("Manual Control: Takeover Noted")
self._takeovers += 1
self._input_control = "Manual"
if keys[K_n]:
print("AI Control")
self._input_control = "AI"
if keys[K_b]:
print("Replay Control")
self._input_control = "Replay"
if keys[K_SPACE]:
control.hand_brake = True
if keys[K_q]:
self._is_on_reverse = not self._is_on_reverse
time.sleep(0.05)
if keys[K_p]:
self._enable_autopilot = not self._enable_autopilot
control.reverse = self._is_on_reverse
return control
def run_carla_client(args):
# Here we will run args._episode episodes with args._frames frames each.
number_of_episodes = args._episode
frames_per_episode = args._frames
# set speed and yaw rate variable to default
speedyawrate = np.nan
#call init in eval file to load model
checkpoint_dir_loc = args.chckpt_loc
prefix = args.model_name
tf_carla_eval.init(checkpoint_dir_loc, prefix)
# create the carla client
with make_carla_client(args.host, args.port, timeout=100000) as client:
print('CarlaClient connected')
for episode in range(0, number_of_episodes):
if args.settings_filepath is None:
# if same starting position arg set use same weather
if args._sameStart > -1:
choice = 1
nrVehicles = 0
nrPedestrians = 0
else:
choice = random.choice([1, 3, 7, 8, 14])
nrVehicles = 150
nrPedestrians = 100
settings = CarlaSettings()
settings.set(SynchronousMode=args.synchronous_mode,
SendNonPlayerAgentsInfo=True,
NumberOfVehicles=nrVehicles,
NumberOfPedestrians=nrPedestrians,
WeatherId=choice,
QualityLevel=args.quality_level)
settings.randomize_seeds()
camera0 = Camera('CameraRGB')
camera0.set_image_size(1920, 640)
camera0.set_position(2.00, 0, 1.30)
settings.add_sensor(camera0)
camera1 = Camera('CameraDepth', PostProcessing='Depth')
camera1.set_image_size(1920, 640)
camera1.set_position(2.00, 0, 1.30)
settings.add_sensor(camera1)
camera2 = Camera('CameraSegmentation',
PostProcessing='SemanticSegmentation')
camera2.set_image_size(1920, 640)
camera2.set_position(2.00, 0, 1.30)
settings.add_sensor(camera2)
if args._save:
camera3 = Camera('CameraRGB_top',
PostProcessing='SceneFinal')
camera3.set_image_size(800, 800)
camera3.set_position(-6.0, 0, 4.0)
camera3.set_rotation(yaw=0, roll=0, pitch=-10)
settings.add_sensor(camera3)
else:
with open(args.settings_filepath, 'r') as fp:
settings = fp.read()
scene = client.load_settings(settings)
# Choose one player start at random.
number_of_player_starts = len(scene.player_start_spots)
if args._sameStart > -1:
player_start = args._sameStart
else:
player_start = random.randint(
0, max(0, number_of_player_starts - 1))
print('Starting new episode at %r...' % scene.map_name)
client.start_episode(player_start)
if args._save:
# initialize stuck variable. if car does not move after colliding for x frames we restart episode.
nrStuck = 0
# maximum number of times we can get stuck before restarting
maxStuck = 5
# last location variable to keep track if car is changing position
last_loc = namedtuple('last_loc', 'x y z')
last_loc.__new__.__defaults__ = (0.0, 0.0, 0.0)
last_loc.x = -1
last_loc.y = -1
# delete frames of previous run to not interfere with video creation
for rmf in glob.glob(args.file_loc + 'tmpFrameFolder/frame*'):
os.remove(rmf)
# Iterate every frame in the episode.
for frame in range(0, frames_per_episode):
measurements, sensor_data = client.read_data()
# if we are saving video of episode move to next episode if we get stuck
if args._save:
player_measurements = measurements.player_measurements
col_other = player_measurements.collision_other
col_cars = player_measurements.collision_vehicles
curr_loc = player_measurements.transform.location
if player_measurements.forward_speed <= 0.05 and sqrdist(
last_loc, curr_loc) < 2 and frame > 30:
if nrStuck == maxStuck:
print(
"\nWe are stuck! Writing to video and restarting."
)
#args._sameStart += 1
break
else:
print("Stuck: " + str(nrStuck) + "/" +
str(maxStuck))
nrStuck += 1
last_loc = curr_loc
# Skip first couple of images due to setup time.
if frame > 19:
player_measurements = measurements.player_measurements
for name, curr_measurements in sensor_data.items():
if name == 'CameraDepth':
depth = curr_measurements.return_depth_map()
if name == 'CameraSegmentation':
segmentation = curr_measurements.return_segmentation_map(
)
if name == 'CameraRGB':
rgb = curr_measurements.return_rgb()
yaw, yaw_rate, speed, prev_time = process_odometry(
measurements, yaw_shift, yaw_old, prev_time)
yaw_old = yaw
im = Image.new('L', (256 * 6, 256 * 6), (127))
shift = 256 * 6 / 2
draw = ImageDraw.Draw(im)
gmTime = time.time()
for agent in measurements.non_player_agents:
if agent.HasField('vehicle') or agent.HasField(
'pedestrian'):
participant = agent.vehicle if agent.HasField(
'vehicle') else agent.pedestrian
if not participantInRange(
participant.transform.location,
player_measurements.transform.location):
continue
angle = cart2pol(
participant.transform.orientation.x,
participant.transform.orientation.y)
polygon = agent2world(participant, angle)
polygon = world2player(
polygon, math.radians(-yaw),
player_measurements.transform)
polygon = player2image(polygon,
shift,
multiplier=25)
polygon = [tuple(row) for row in polygon.T]
draw.polygon(polygon, 0, 0)
im = im.resize((256, 256), Image.ANTIALIAS)
im = im.rotate(imrotate)
gmTime = time.time() - gmTime
if not args.all_data:
print("only all data supported for now")
exit()
else:
if args._opd:
ppTime, evTime, imTime, tkTime, speedyawrate, direction = tf_carla_eval.eval_only_drive(
im, rgb, depth, segmentation, -yaw_rate, speed)
else:
ppTime, evTime, imTime, tkTime, speedyawrate, direction, imReady, im = tf_carla_eval.eval(
im, rgb, depth, segmentation, -yaw_rate, speed)
if args._save and imReady:
# append frame to array for video output
for name, curr_measurements in sensor_data.items(
):
if name == 'CameraRGB_top':
filename = args.file_loc + "tmpFrameFolder/frame" + str(
frame).zfill(4) + ".jpg"
Image.fromarray(
np.concatenate([
im,
curr_measurements.return_rgb()
], 1)).save(filename)
break
#printTimePerEval(gmTime, ppTime, evTime, imTime, tkTime)
else:
# get first values
yaw_shift = measurements.player_measurements.transform.rotation.yaw
yaw_old = ((yaw_shift - 180) % 360) - 180
imrotate = round(yaw_old) + 90
shift_x = measurements.player_measurements.transform.location.x
shift_y = measurements.player_measurements.transform.location.y
prev_time = np.int64(measurements.game_timestamp)
if not args.autopilot:
control = getKeyboardControl()
else:
# values are nan while script is gathering frames before first prediction
if np.any(np.isnan(speedyawrate)):
control = measurements.player_measurements.autopilot_control
control.steer += random.uniform(-0.01, 0.01)
else:
control = VehicleControl()
# speedyawrate contains T entries, first entry is first prediction
dirAvgEncoding = np.mean(
np.squeeze(np.array(direction)), 0)
speedyawrate = np.asarray(speedyawrate)
steerAvgEncoding = np.mean(np.squeeze(speedyawrate), 0)
control.throttle = mapThrottle(player_measurements,
speedyawrate)
control.brake = int(control.throttle == -1)
control.steer = mapSteer(steerAvgEncoding[1])
#printSteering(measurements.player_measurements.forward_speed,
# measurements.player_measurements.autopilot_control.throttle,
# measurements.player_measurements.autopilot_control.steer,
# speedyawrate, control.throttle, control.steer,
# dirAvgEncoding, steerAvgEncoding)
client.send_control(control)
if args._save:
print("\nConverting frames to video...")
os.system(
"ffmpeg -r 10 -f image2 -start_number 20 -i {}frame%04d.jpg -vcodec libx264 -crf 15 -pix_fmt yuv420p {}"
.format(
args.file_loc + "tmpFrameFolder/", args.file_loc +
"video" + str(time.time() * 1000) + ".mp4"))
print("Finished conversion.")
