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)
