def get_vehicle_state(self): """Get the vehicle state as an ndarray. State consists of [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z, length, width, height, pitch, yaw, roll] where pitch, yaw, roll are in radians.""" return carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( self.__ego_vehicle)
def do_clustering(self, frame): pred_result = self.do_prediction(frame) ovehicles = self.make_ovehicles(pred_result) params = self.make_local_params(frame, ovehicles) vertices = self.compute_vertices(params, ovehicles) A_union, b_union = self.compute_overapproximations( params, ovehicles, vertices) states = [] bboxes = [] for vehicle in ovehicles: bboxes.append(vehicle.bbox) if vehicle.node.id == "ego": vehicle = self.__ego_vehicle else: vehicle = self.__other_vehicles[int(vehicle.node.id)] state = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray(vehicle, flip_y=True) state = np.array( [state[0], state[1], state[13], get_speed(vehicle)]) states.append(state) states = np.stack(states) bboxes = np.stack(bboxes) self.__plot_simulation_data.states[frame] = states self.__plot_simulation_data.bboxes[frame] = bboxes self.__plot_simulation_data.vertices[frame] = vertices O, K = params.O, params.K self.__plot_simulation_data.OK_Ab_union[frame] = (O, K, A_union, b_union)
def run_step(self, frame, control=None): """Run motion planner step. Should be called whenever carla.World.click() is called. Parameters ========== frame : int Current frame of the simulation. control: carla.VehicleControl (optional) Optional control to apply to the motion planner. Used to move the vehicle while burning frames in the simulator before doing motion planning. """ logging.debug(f"In LCSSHighLevelAgent.run_step() with frame = {frame}") if self.__first_frame is None: self.do_first_step(frame) self.__scene_builder.capture_trajectory(frame) if (frame - self.__first_frame) % self.__scene_config.record_interval == 0: """We only motion plan every `record_interval` frames (e.g. every 0.5 seconds of simulation).""" frame_id = int( (frame - self.__first_frame) / self.__scene_config.record_interval ) if frame_id < self.__n_burn_interval: """Initially collect data without doing any control to the vehicle.""" pass elif (frame_id - self.__n_burn_interval) % self.__step_horizon == 0: speeds, angles = self.__compute_prediction_controls(frame) self.__local_planner.set_plan( speeds, angles, self.__scene_config.record_interval ) if self.plot_simulation: """Save actual trajectory for final plotting""" payload = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( self.__ego_vehicle, flip_y=True ) payload = np.array( [ payload[0], payload[1], payload[13], self.get_current_velocity(), ] ) self.__plot_simulation_data.actual_trajectory[frame] = payload self.__plot_simulation_data.goals[frame] = self.get_goal() if not control: control = self.__local_planner.step() self.__ego_vehicle.apply_control(control) if self.plot_simulation: payload = self.__local_planner.get_current() self.__plot_simulation_data.lowlevel[frame] = payload
def __capture_agents_within_radius(self, frame_id): labels = self.__map_reader.get_actor_labels(self.__ego_vehicle) if self.__should_exclude_dataset_sample(labels): self.__remove_scene_builder() player_location = carlautil.to_location_ndarray(self.__ego_vehicle) player_z = player_location[2] if len(self.__other_vehicles): other_ids = list(self.__other_vehicles.keys()) other_vehicles = self.__other_vehicles.values() others_data = carlautil.actors_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( other_vehicles).T other_locations = others_data[:3].T distances = np.linalg.norm(other_locations - player_location, axis=1) df = pd.DataFrame({ "frame_id": np.full((len(other_ids), ), frame_id), "type": [self.__scene_config.node_type.VEHICLE] * len(other_ids), "node_id": util.map_to_list(str, other_ids), "robot": [False] * len(other_ids), "distances": distances, "x": others_data[0], "y": others_data[1], "z": others_data[2], "v_x": others_data[3], "v_y": others_data[4], "v_z": others_data[5], "a_x": others_data[6], "a_y": others_data[7], "a_z": others_data[8], "length": others_data[9], "width": others_data[10], "height": others_data[11], "heading": others_data[13], }) df = df[df["distances"] < self.__radius] df = df[df["z"].between( player_z - self.Z_LOWERBOUND, player_z + self.Z_UPPERBOUND, inclusive="neither", )] del df["distances"] else: df = pd.DataFrame(columns=[ "frame_id", "type", "node_id", "robot", "x", "y", "z", "length", "width", "height", "heading", ]) ego_data = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray( self.__ego_vehicle) data_point = pd.DataFrame.from_records([{ "frame_id": frame_id, "type": self.__scene_config.node_type.VEHICLE, "node_id": "ego", "robot": True, "x": ego_data[0], "y": ego_data[1], "z": ego_data[2], "v_x": ego_data[3], "v_y": ego_data[4], "v_z": ego_data[5], "a_x": ego_data[6], "a_y": ego_data[7], "a_z": ego_data[8], "length": ego_data[9], "width": ego_data[10], "height": ego_data[11], "heading": ego_data[13], }]) df = pd.concat((df, data_point), ignore_index=True) # df = df.append(data_point, ignore_index=True) if self.__trajectory_data is None: self.__trajectory_data = df else: self.__trajectory_data = pd.concat((self.__trajectory_data, df), ignore_index=True)