def decode_control(self, cod): control = VehicleControl() control.steer = 0 control.throttle = 0 control.brake = 0 control.hand_brake = False control.reverse = False if cod > 9: control.hand_brake = True if cod == 10: control.steer = -1 elif cod == 10: 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 action_joystick(self): # joystick steering_axis = self.joystick.get_axis(0) acc_axis = self.joystick.get_axis(2) brake_axis = self.joystick.get_axis(5) # print("axis 0 %f, axis 2 %f, axis 3 %f" % (steering_axis, acc_axis, brake_axis)) if (self.joystick.get_button(3)): self._rear = True if (self.joystick.get_button(2)): self._rear = False control = VehicleControl() control.steer = steering_axis control.throttle = (acc_axis + 1) / 2.0 control.brake = (brake_axis + 1) / 2.0 if control.brake < 0.001: control.brake = 0.0 control.hand_brake = 0 control.reverse = self._rear control.steer -= 0.0822 #print("steer %f, throttle %f, brake %f" % (control.steer, control.throttle, control.brake)) pygame.event.pump() return control
def _compute_action(self, rgb_image, speed, directions=None): #shit for demo img = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) cv2.imwrite(self.img_dir + "/image_" + str(self.nr_img) + ".jpg", img) #shit for demo rgb_image = rgb_image[self._image_cut[0]:self._image_cut[1], :] steer, acc, brake = self._control_function(rgb_image, speed, directions) # 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: acc = 0.0 control = VehicleControl() control.steer = steer control.throttle = acc control.brake = brake control.hand_brake = 0 control.reverse = 0 return 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 _control_using_agent(self, action): ''' Here we pass the action output by our agent and control the car outputs : As of now consider the agent also outputs the keys W, A, S, D, space, reverse so according to the output vector "action" For example : action = [1, 1, 0, 0, 0, 0] => it uses throttle and steer left action = [0, 0, 0, 0, 0, 1] => car will be on reverse gear ''' '''To be completed''' # if action[6]: # # if r is pressed # return None control = VehicleControl() if action[1]: control.steer = -1.0 if action[3]: control.steer = 1.0 if action[0]: control.throttle = 1.0 if action[2]: control.brake = 1.0 if action[4]: control.hand_brake = True if action[5]: self._is_on_reverse = not self._is_on_reverse 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 _get_keyboard_control(self, keys, args): """ Return a VehicleControl message based on the pressed keys. Return None if a new episode was requested. """ global flg_warning 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 if keys[K_p]: self._enable_autopilot = not self._enable_autopilot if args.app == 'Control': if flg_warning == -1: control.throttle = 0.0 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 _get_keyboard_control(self, keys, measurements): """ Return a VehicleControl message based on the pressed keys. Return None if a new episode was requested. """ if keys[K_p]: control = measurements.player_measurements.autopilot_control control.steer += random.uniform(-0.1, 0.1) return control 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_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, None if keys[K_t]: self.should_display = not self.should_display return 'done', None if keys[K_m]: self.manual = True return 'done', None if keys[K_n]: self.manual = False return 'done', None if keys[K_v]: self.endnow = True return 'done', 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 if keys[K_c]: self.manual_control = not self.manual_control if keys[K_p]: self._enable_autopilot = not self._enable_autopilot control.reverse = self._is_on_reverse reward = None if keys[K_1]: reward = -1 if keys[K_2]: reward = -0.5 if keys[K_3]: reward = -0.25 if keys[K_4]: reward = -0.1 if keys[K_5]: reward = 0 if keys[K_6]: reward = 0.1 if keys[K_7]: reward = 0.25 if keys[K_8]: reward = 0.5 if keys[K_9]: reward = 1 return control, reward
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 _get_keyboard_control(self, keys, measurements, sensor_data): """ Return a VehicleControl message based on the pressed keys. Return None if a new episode was requested. """ self._at_intersection = False if keys[K_r]: return None if self._NNagent_drives: if keys[K_LEFT] or keys[K_a]: control = self.NNagent.run_step(measurements, sensor_data, 3, None) elif keys[K_RIGHT] or keys[K_d]: control = self.NNagent.run_step(measurements, sensor_data, 4, None) elif keys[K_UP] or keys[K_w]: control = self.NNagent.run_step(measurements, sensor_data, 2, None) else: control = self.NNagent.run_step(measurements, sensor_data, -1, None) if keys[K_q]: self._is_on_reverse = not self._is_on_reverse if keys[K_p]: self._enable_autopilot = True self._NNagent_drives = not self._NNagent_drives if keys[K_n]: print("Turning off") self._NNagent_drives = not self._NNagent_drives if keys[K_i]: self._at_intersection = True control.reverse = self._is_on_reverse else: 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 if keys[K_p]: self._enable_autopilot = not self._enable_autopilot if keys[K_n]: print("Turning on") self._NNagent_drives = not self._NNagent_drives self._enable_autopilot = False if keys[K_i]: self._at_intersection = True control.reverse = self._is_on_reverse 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_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_keyboard_control(self, keys): control = VehicleControl() if keys[pl.K_LEFT] or keys[pl.K_a]: control.steer = -1.0 if keys[pl.K_RIGHT] or keys[pl.K_d]: control.steer = 1.0 if keys[pl.K_UP] or keys[pl.K_w]: control.throttle = 1.0 if keys[pl.K_DOWN] or keys[pl.K_s]: control.brake = 1.0 if keys[pl.K_SPACE]: control.hand_brake = True control.reverse = self._vehicle_in_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 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 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_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() 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_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 _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_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 throw_brake(self): control = VehicleControl() control.hand_brake = True self.client.send_control(control)
def run_carla_client(host, port, far): # Here we will run a single episode with 300 frames. number_of_frames = 30 frame_step = 10 # Save one image every 100 frames image_size = [800, 600] camera_local_pos = [0.3, 0.0, 1.3] # [X, Y, Z] camera_local_rotation = [0, 0, 0] # [pitch(Y), yaw(Z), roll(X)] fov = 70 autopilot = False control = VehicleControl() for start_i in range(150): output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str( start_i) if not os.path.exists(output_folder): os.mkdir(output_folder) print("make " + str(output_folder)) # Connect with the server with make_carla_client(host, port) as client: print('CarlaClient connected') for start_i in range(150): output_folder = '/home2/mariap/Packages/CARLA_0.8.2/PythonClient/_out/pos_' + str( start_i) print(output_folder) # Here we load the settings. settings = CarlaSettings() settings.set(SynchronousMode=True, SendNonPlayerAgentsInfo=True, NumberOfVehicles=100, NumberOfPedestrians=500, WeatherId=random.choice([1, 3, 7, 8, 14])) settings.randomize_seeds() camera1 = Camera('CameraDepth', PostProcessing='Depth', FOV=fov) camera1.set_image_size(*image_size) camera1.set_position(*camera_local_pos) camera1.set_rotation(*camera_local_rotation) settings.add_sensor(camera1) camera2 = Camera('CameraRGB', PostProcessing='SceneFinal', FOV=fov) camera2.set_image_size(*image_size) camera2.set_position(*camera_local_pos) camera2.set_rotation(*camera_local_rotation) settings.add_sensor(camera2) camera3 = Camera('CameraSeg', PostProcessing='SemanticSegmentation') camera3.set_image_size(*image_size) camera3.set_position(*camera_local_pos) camera3.set_rotation(*camera_local_rotation) settings.add_sensor(camera3) client.load_settings(settings) # Start at location index id '0' client.start_episode(start_i) cameras_dict = {} pedestrians_dict = {} cars_dict = {} # Compute the camera transform matrix camera_to_car_transform = camera2.get_unreal_transform() # (Intrinsic) (3, 3) K Matrix K = np.identity(3) K[0, 2] = image_size[0] / 2.0 K[1, 2] = image_size[1] / 2.0 K[0, 0] = K[1, 1] = image_size[0] / (2.0 * np.tan(fov * np.pi / 360.0)) with open(output_folder + '/camera_intrinsics.p', 'w') as camfile: pickle.dump(K, camfile) # Iterate every frame in the episode except for the first one. for frame in range(1, number_of_frames): # Read the data produced by the server this frame. measurements, sensor_data = client.read_data() # Save one image every 'frame_step' frames if not frame % frame_step: for name, measurement in sensor_data.items(): filename = '{:s}/{:0>6d}'.format(name, frame) measurement.save_to_disk( os.path.join(output_folder, filename)) # Start transformations time mesure. timer = StopWatch() # RGB image [[[r,g,b],..[r,g,b]],..[[r,g,b],..[r,g,b]]] image_RGB = to_rgb_array(sensor_data['CameraRGB']) image_seg = np.tile( labels_to_array(sensor_data['CameraSeg']), (1, 1, 3)) # 2d to (camera) local 3d # We use the image_RGB to colorize each 3D point, this is optional. # "max_depth" is used to keep only the points that are near to the # camera, meaning 1.0 the farest points (sky) point_cloud = depth_to_local_point_cloud( sensor_data['CameraDepth'], image_RGB, max_depth=far) point_cloud_seg = depth_to_local_point_cloud( sensor_data['CameraDepth'], image_seg, max_depth=far) # (Camera) local 3d to world 3d. # Get the transform from the player protobuf transformation. world_transform = Transform( measurements.player_measurements.transform) # Compute the final transformation matrix. car_to_world_transform = world_transform * camera_to_car_transform # Car to World transformation given the 3D points and the # transformation matrix. point_cloud.apply_transform(car_to_world_transform) point_cloud_seg.apply_transform(car_to_world_transform) Rt = car_to_world_transform.matrix Rt_inv = car_to_world_transform.inverse( ).matrix # Rt_inv is the world to camera matrix !! #R_inv=world_transform.inverse().matrix cameras_dict[frame] = {} cameras_dict[frame]['inverse_rotation'] = Rt_inv[:] cameras_dict[frame]['rotation'] = Rt[:] cameras_dict[frame]['translation'] = Rt_inv[0:3, 3] cameras_dict[frame][ 'camera_to_car'] = camera_to_car_transform.matrix # Get non-player info vehicles = {} pedestrians = {} for agent in measurements.non_player_agents: # check if the agent is a vehicle. if agent.HasField('vehicle'): pos = agent.vehicle.transform.location pos_vector = np.array([[pos.x], [pos.y], [pos.z], [1.0]]) trnasformed_3d_pos = np.dot(Rt_inv, pos_vector) pos2d = np.dot(K, trnasformed_3d_pos[:3]) # Normalize the point norm_pos2d = np.array([ pos2d[0] / pos2d[2], pos2d[1] / pos2d[2], pos2d[2] ]) # Now, pos2d contains the [x, y, d] values of the image in pixels (where d is the depth) # You can use the depth to know the points that are in front of the camera (positive depth). x_2d = image_size[0] - norm_pos2d[0] y_2d = image_size[1] - norm_pos2d[1] vehicles[agent.id] = {} vehicles[agent.id]['transform'] = [ agent.vehicle.transform.location.x, agent.vehicle.transform.location.y, agent.vehicle.transform.location.z ] vehicles[agent.id][ 'bounding_box.transform'] = agent.vehicle.bounding_box.transform.location.z vehicles[agent.id][ 'yaw'] = agent.vehicle.transform.rotation.yaw vehicles[agent.id]['bounding_box'] = [ agent.vehicle.bounding_box.extent.x, agent.vehicle.bounding_box.extent.y, agent.vehicle.bounding_box.extent.z ] vehicle_transform = Transform( agent.vehicle.bounding_box.transform) pos = agent.vehicle.transform.location bbox3d = agent.vehicle.bounding_box.extent # Compute the 3D bounding boxes # f contains the 4 points that corresponds to the bottom f = np.array([[ pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z - bbox3d.z + agent.vehicle.bounding_box.transform.location.z ], [ pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z - bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z - bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z - bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z + agent.vehicle. bounding_box.transform.location.z ], [ pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z + agent.vehicle. bounding_box.transform.location.z ]]) f_rotated = vehicle_transform.transform_points(f) f_2D_rotated = [] vehicles[ agent.id]['bounding_box_coord'] = f_rotated for i in range(f.shape[0]): point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]]) transformed_2d_pos = np.dot( Rt_inv, point) # 3d Position in camera space pos2d = np.dot( K, transformed_2d_pos[:3] ) # Conversion to camera frustum space norm_pos2d = np.array([ pos2d[0] / pos2d[2], pos2d[1] / pos2d[2], pos2d[2] ]) #print([image_size[0] - (pos2d[0] / pos2d[2]), image_size[1] - (pos2d[1] / pos2d[2])]) f_2D_rotated.append( np.array([ image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1] ])) vehicles[ agent.id]['bounding_box_2D'] = f_2D_rotated elif agent.HasField('pedestrian'): pedestrians[agent.id] = {} pedestrians[agent.id]['transform'] = [ agent.pedestrian.transform.location.x, agent.pedestrian.transform.location.y, agent.pedestrian.transform.location.z ] pedestrians[agent.id][ 'yaw'] = agent.pedestrian.transform.rotation.yaw pedestrians[agent.id]['bounding_box'] = [ agent.pedestrian.bounding_box.extent.x, agent.pedestrian.bounding_box.extent.y, agent.pedestrian.bounding_box.extent.z ] vehicle_transform = Transform( agent.pedestrian.bounding_box.transform) pos = agent.pedestrian.transform.location bbox3d = agent.pedestrian.bounding_box.extent # Compute the 3D bounding boxes # f contains the 4 points that corresponds to the bottom f = np.array( [[pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z], [pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z], [pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z], [pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z], [ pos.x + bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z ], [ pos.x + bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z ], [ pos.x - bbox3d.x, pos.y + bbox3d.y, pos.z + bbox3d.z ], [ pos.x - bbox3d.x, pos.y - bbox3d.y, pos.z + bbox3d.z ]]) f_rotated = vehicle_transform.transform_points(f) pedestrians[ agent.id]['bounding_box_coord'] = f_rotated f_2D_rotated = [] for i in range(f.shape[0]): point = np.array([[f_rotated[i, 0]], [f_rotated[i, 1]], [f_rotated[i, 2]], [1]]) transformed_2d_pos = np.dot( Rt_inv, point) # See above for cars pos2d = np.dot(K, transformed_2d_pos[:3]) norm_pos2d = np.array([ pos2d[0] / pos2d[2], pos2d[1] / pos2d[2], pos2d[2] ]) f_2D_rotated.append([ image_size[0] - norm_pos2d[0], image_size[1] - norm_pos2d[1] ]) pedestrians[ agent.id]['bounding_box_2D'] = f_2D_rotated cars_dict[frame] = vehicles pedestrians_dict[frame] = pedestrians # End transformations time mesure. timer.stop() # Save PLY to disk # This generates the PLY string with the 3D points and the RGB colors # for each row of the file. point_cloud.save_to_disk( os.path.join(output_folder, '{:0>5}.ply'.format(frame))) point_cloud_seg.save_to_disk( os.path.join(output_folder, '{:0>5}_seg.ply'.format(frame))) print_message(timer.milliseconds(), len(point_cloud), frame) if autopilot: client.send_control( measurements.player_measurements.autopilot_control) else: control.hand_brake = True client.send_control(control) with open(output_folder + '/cameras.p', 'w') as camerafile: pickle.dump(cameras_dict, camerafile) print(output_folder + "cameras.p") with open(output_folder + '/people.p', 'w') as peoplefile: pickle.dump(pedestrians_dict, peoplefile) with open(output_folder + '/cars.p', 'w') as carfile: pickle.dump(cars_dict, carfile)