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): #将数字的control转换为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.")