def __plot_scenario(self, pred_result, ovehicles, params, ctrl_result):
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control"
if self.__agent_type == "oa":
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_lcss_prediction(pred_result,
ovehicles,
params,
ctrl_result,
self.__prediction_horizon,
ego_bbox,
filename=filename)
elif self.__agent_type == "mcc":
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_multiple_coinciding_controls(pred_result,
ovehicles,
params,
ctrl_result,
ego_bbox,
filename=filename)
elif self.__agent_type == "rmcc":
pass
else:
raise NotImplementedError()
def __plot_simulation(self):
if len(self.__plot_simulation_data.planned_trajectories) == 0:
return
if self.__agent_type == "oa":
filename = f"agent{self.__ego_vehicle.id}_oa_simulation"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
plot_oa_simulation(
self.__scene_builder.get_scene(),
self.__plot_simulation_data.actual_trajectory,
self.__plot_simulation_data.planned_trajectories,
self.__plot_simulation_data.planned_controls,
self.__road_segs, [lon, lat],
self.__control_horizon,
filename=filename)
elif self.__agent_type == "mcc":
filename = f"agent{self.__ego_vehicle.id}_mcc_simulation"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
plot_multiple_simulation(
self.__scene_builder.get_scene(),
self.__plot_simulation_data.actual_trajectory,
self.__plot_simulation_data.planned_trajectories,
self.__plot_simulation_data.planned_controls,
self.__road_segs, [lon, lat],
self.__control_horizon,
filename=filename)
else:
raise NotImplementedError()
def __make_global_params(self):
"""Get global parameters used across all loops"""
params = util.AttrDict()
# Big M variable for Slack variables in solver
params.M = 1000
# Control variable for solver, setting max acceleration
params.max_a = 2.5
# Maximum steering angle
physics_control = self.__ego_vehicle.get_physics_control()
wheels = physics_control.wheels
params.max_delta = np.deg2rad(wheels[0].max_steer_angle)
# longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp.
params.bbox_lon, params.bbox_lat, _ = carlautil.actor_to_bbox_ndarray(
self.__ego_vehicle)
params.diag = np.sqrt(params.bbox_lon**2 + params.bbox_lat**2) / 2.
params.A, params.B = self.__get_state_space_representation(
self.__timestep)
# number of state variables x, number of input variables u
# nx = 4, nu = 2
params.nx, params.nu = params.B.shape
# Closed for solution of control without obstacles
A, B, nx, nu = params.A, params.B, params.nx, params.nu
T = self.__control_horizon
# C1 has shape (nx, T*nx)
C1 = np.zeros((
nx,
T * nx,
))
# C2 has shape (nx*(T - 1), nx*(T-1)) as A has shape (nx, nx)
C2 = np.kron(np.eye(T - 1), A)
# C3 has shape (nx*(T - 1), nx)
C3 = np.zeros((
(T - 1) * nx,
nx,
))
# C, Abar have shape (nx*T, nx*T)
C = np.concatenate((
C1,
np.concatenate((
C2,
C3,
), axis=1),
), axis=0)
Abar = np.eye(T * nx) - C
# Bbar has shape (nx*T, nu*T) as B has shape (nx, nu)
Bbar = np.kron(np.eye(T), B)
# Gamma has shape (nx*(T + 1), nu*T) as Abar\Bbar has shape (nx*T, nu*T)
Gamma = np.concatenate((
np.zeros((
nx,
T * nu,
)),
np.linalg.solve(Abar, Bbar),
))
params.Abar = Abar
params.Bbar = Bbar
params.Gamma = Gamma
return params
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_highlevel_params(ovehicles)
ctrl_result = self.do_highlevel_control(params, ovehicles)
"""Get trajectory"""
trajectory = []
x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle)
X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star))
n_steps = X.shape[0]
headings = []
for t in range(1, n_steps):
x, y = X[t]
y = -y
yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
headings.append(yaw)
yaw = util.reflect_radians_about_x_axis(yaw)
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
"""Plot scenario"""
ctrl_result.headings = headings
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
plot_lcss_prediction(pred_result, ovehicles, params, ctrl_result,
self.__prediction_horizon, ego_bbox)
return trajectory
def __plot_scenario(self,
pred_result,
ovehicles,
params,
ctrl_result,
error=None):
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = np.array([lon, lat])
params.update(self.__params)
if error:
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_mcc_fail"
PlotPredictiveControl(
pred_result,
ovehicles,
params,
ctrl_result,
self.__control_horizon,
ego_bbox,
T_coin=self.__n_coincide).plot_mcc_failure(filename=filename)
else:
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_mcc_predict"
PlotPredictiveControl(
pred_result,
ovehicles,
params,
ctrl_result,
self.__control_horizon,
ego_bbox,
T_coin=self.__n_coincide).plot_mcc_prediction(
filename=filename)
def __make_global_params(self):
"""Get scenario wide parameters used across all loops"""
params = util.AttrDict()
# Slack variable for solver
params.M_big = 10_000
# Control variable for solver, setting max/min acceleration/speed
params.max_a = 1
params.min_a = -7
params.max_v = 5
# objective : util.AttrDict
# Parameters in objective function.
params.objective = util.AttrDict(w_final=2.,
w_ch_accel=0.5,
w_ch_turning=0.5,
w_accel=0.5,
w_turning=0.5)
# Maximum steering angle
physics_control = self.__ego_vehicle.get_physics_control()
wheels = physics_control.wheels
params.limit_delta = np.deg2rad(wheels[0].max_steer_angle)
# Max steering
# We fix max turning angle to make reasonable planned turns.
params.max_delta = 0.5 * params.limit_delta
# longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp.
bbox = util.AttrDict()
bbox.lon, bbox.lat, _ = carlautil.actor_to_bbox_ndarray(
self.__ego_vehicle)
params.bbox = bbox
# Number of faces of obstacle sets
params.L = 4
# Minimum distance from vehicle to avoid collision.
# Assumes that car is a circle.
# TODO: remove this. Improve bounds instead
params.diag = np.sqrt(bbox.lon**2 + bbox.lat**2) / 2.
return params
def make_ovehicles(self, result):
scene, timestep, nodes = result.scene, result.timestep, result.nodes
predictions, latent_probs, z = result.predictions, result.latent_probs, result.z
past_dict, ground_truth_dict = result.past_dict, result.ground_truth_dict
"""Preprocess predictions"""
minpos = np.array([scene.x_min, scene.y_min])
ovehicles = []
for idx, node in enumerate(nodes):
if node.id == 'ego':
continue
lon, lat, _ = carlautil.actor_to_bbox_ndarray(
self.__other_vehicles[int(node.id)])
veh_bbox = [lon, lat]
veh_gt = ground_truth_dict[timestep][node] + minpos
veh_past = past_dict[timestep][node] + minpos
veh_predict = predictions[idx] + minpos
veh_latent_pmf = latent_probs[idx]
n_states = veh_latent_pmf.size
zn = z[idx]
veh_latent_predictions = [[] for x in range(n_states)]
for jdx, p in enumerate(veh_predict):
veh_latent_predictions[zn[jdx]].append(p)
for jdx in range(n_states):
veh_latent_predictions[jdx] = np.array(
veh_latent_predictions[jdx])
ovehicle = OVehicle.from_trajectron(node,
self.__prediction_horizon,
veh_gt,
veh_past,
veh_latent_pmf,
veh_latent_predictions,
bbox=veh_bbox)
ovehicles.append(ovehicle)
return ovehicles
def make_ovehicles(self, result):
scene, timestep, nodes = result.scene, result.timestep, result.nodes
predictions, latent_probs, z = result.predictions, result.latent_probs, result.z
past_dict, ground_truth_dict = result.past_dict, result.ground_truth_dict
"""Preprocess predictions"""
minpos = np.array([scene.x_min, scene.y_min])
ovehicles = []
for idx, node in enumerate(nodes):
if node.id == "ego":
lon, lat, _ = carlautil.actor_to_bbox_ndarray(
self.__ego_vehicle)
else:
lon, lat, _ = carlautil.actor_to_bbox_ndarray(
self.__other_vehicles[int(node.id)])
veh_bbox = np.array([lon, lat])
veh_gt = ground_truth_dict[timestep][node] + minpos
veh_past = past_dict[timestep][node] + minpos
veh_predict = predictions[idx] + minpos
veh_latent_pmf = latent_probs[idx]
n_states = veh_latent_pmf.size
zn = z[idx]
veh_latent_predictions = [[] for x in range(n_states)]
for jdx, p in enumerate(veh_predict):
veh_latent_predictions[zn[jdx]].append(p)
for jdx in range(n_states):
veh_latent_predictions[jdx] = np.array(
veh_latent_predictions[jdx])
ovehicle = OVehicle.from_trajectron(
node,
self.__prediction_horizon,
veh_gt,
veh_past,
veh_latent_pmf,
veh_latent_predictions,
bbox=veh_bbox,
)
# if node.id != "ego":
# vehicle = self.__other_vehicles[int(node.id)]
# pos = carlautil.to_location_ndarray(vehicle, flip_y=True)
# out = f"id: {node.id}, pos: {pos}, last: {ovehicle.past[-1]}, gt: {ovehicle.ground_truth}"
# logging.info(out)
ovehicles.append(ovehicle)
return ovehicles
def __make_global_params(self):
"""Get Global LCSS parameters used across all loops"""
params = util.AttrDict()
params.M_big = 1000
params.u_max = 3.
params.A, params.B = self.__get_state_space_representation(
self.__prediction_timestep)
# number of state variables x, number of input variables u
# nx = 4, nu = 2
params.nx, params.nu = params.B.shape
# longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp.
bbox_lon, bbox_lat, _ = carlautil.actor_to_bbox_ndarray(
self.__ego_vehicle)
params.diag = np.sqrt(bbox_lon**2 + bbox_lat**2) / 2.
# Prediction parameters
params.T = self.__prediction_horizon
params.L = 4 # number of faces of obstacle sets
# Closed for solution of control without obstacles
A, B, T, nx, nu = params.A, params.B, params.T, params.nx, params.nu
# C1 has shape (nx, T*nx)
C1 = np.zeros((
nx,
T * nx,
))
# C2 has shape (nx*(T - 1), nx*(T-1)) as A has shape (nx, nx)
C2 = np.kron(np.eye(T - 1), A)
# C3 has shape (nx*(T - 1), nx)
C3 = np.zeros((
(T - 1) * nx,
nx,
))
# C, Abar have shape (nx*T, nx*T)
C = np.concatenate((
C1,
np.concatenate((
C2,
C3,
), axis=1),
), axis=0)
Abar = np.eye(T * nx) - C
# Bbar has shape (nx*T, nu*T) as B has shape (nx, nu)
Bbar = np.kron(np.eye(T), B)
# Gamma has shape (nx*(T + 1), nu*T) as Abar\Bbar has shape (nx*T, nu*T)
Gamma = np.concatenate((
np.zeros((
nx,
T * nu,
)),
np.linalg.solve(Abar, Bbar),
))
params.Abar = Abar
params.Bbar = Bbar
params.Gamma = Gamma
return params
def __plot_simulation(self):
if len(self.__plot_simulation_data.planned_trajectories) == 0:
return
filename = f"agent{self.__ego_vehicle.id}_oa_simulation"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
plot_oa_simulation(self.__scene_builder.get_scene(),
self.__plot_simulation_data.actual_trajectory,
self.__plot_simulation_data.planned_trajectories,
self.__plot_simulation_data.planned_controls,
[lon, lat],
self.__control_horizon,
filename=filename)
def __plot_scenario(self, pred_result, ovehicles, params, ctrl_result):
if self.__agent_type == "oa":
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_lcss_prediction(pred_result,
ovehicles,
params,
ctrl_result,
self.__prediction_horizon,
ego_bbox,
filename=filename)
def __plot_simulation(self):
if len(self.__plot_simulation_data.planned_trajectories) == 0:
return
filename = f"agent{self.__ego_vehicle.id}_oa_simulation"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
plot_oa_simulation(
self.__map_reader.map_data,
self.__plot_simulation_data.actual_trajectory,
self.__plot_simulation_data.planned_trajectories,
self.__plot_simulation_data.planned_controls,
self.__plot_simulation_data.goals,
self.__road_segs, [lon, lat],
self.__step_horizon,
filename=filename,
road_boundary_constraints=self.road_boundary_constraints)
def __make_global_params(self):
"""Get global parameters used across all loops"""
params = util.AttrDict()
# Big M variable for Slack variables in solver
params.M = 1000
# Control variable for solver, setting max acceleration
params.max_a = 2.5
# Maximum steering angle
physics_control = self.__ego_vehicle.get_physics_control()
wheels = physics_control.wheels
params.max_delta = np.deg2rad(wheels[0].max_steer_angle)
# longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp.
params.bbox_lon, params.bbox_lat, _ = carlautil.actor_to_bbox_ndarray(
self.__ego_vehicle)
params.diag = np.sqrt(params.bbox_lon**2 + params.bbox_lat**2) / 2.
return params
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_local_params(ovehicles)
ctrl_result = self.do_highlevel_control(params, ovehicles)
"""use control input next round for warm starting."""
self.U_warmstarting = ctrl_result.U_star
"""Get trajectory"""
trajectory = []
x, y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star[:, :2]))
n_steps = X.shape[0]
headings = []
for t in range(1, n_steps):
x, y = X[t]
y = -y # flip about x-axis again to move back to UE coordinates
yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
headings.append(yaw)
# flip about x-axis again to move back to UE coordinates
yaw = util.reflect_radians_about_x_axis(yaw)
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
if self.plot_scenario:
"""Plot scenario"""
filename = f"agent{self.__ego_vehicle.id}_frame{frame}_lcss_control"
ctrl_result.headings = headings
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_lcss_prediction(pred_result,
ovehicles,
params,
ctrl_result,
self.__prediction_horizon,
ego_bbox,
filename=filename)
if self.plot_simulation:
"""Save planned trajectory for final plotting"""
self.__plot_simulation_data.planned_trajectories[
frame] = np.concatenate((params.x0[None], ctrl_result.X_star))
self.__plot_simulation_data.planned_controls[
frame] = ctrl_result.U_star
return trajectory
def __plot_scenario(
self, pred_result, ovehicles, params, ctrl_result, error=None
):
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = np.array([lon, lat])
params.update(self.__params)
if error:
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_optim_fail"
plot_predictive_control_failure(
pred_result, ovehicles, params, ctrl_result,
self.__control_horizon, ego_bbox, filename=filename
)
else:
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_lcss_control"
plot_lcss_prediction(
pred_result, ovehicles, params, ctrl_result,
self.__control_horizon, ego_bbox, filename=filename
)
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_local_params(ovehicles)
ctrl_result = self.do_highlevel_control(params, ovehicles)
"""Get trajectory"""
# X has shape (T+1, nx)
X = np.concatenate(
(ctrl_result.start[None], ctrl_result.X_star[0, :, :2]))
trajectory = []
for t in range(1, self.__n_coincide + 1):
x, y = X[t]
y = -y # flip about x-axis again to move back to UE coordinates
yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
# flip about x-axis again to move back to UE coordinates
yaw = util.reflect_radians_about_x_axis(yaw)
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
if self.log_agent:
logging.info(
f"Optimized {params.N_select}/{params.N_traj} trajectories avoiding {params.O} vehicles."
)
logging.info(
f"Selected trajectory indices are: {params.subtraj_indices}")
if self.plot_scenario:
"""Plot scenario"""
filename = f"agent{self.__ego_vehicle.id}_frame{frame}_lcss_control"
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_multiple_coinciding_controls(pred_result,
ovehicles,
params,
ctrl_result,
ego_bbox,
filename=filename)
return trajectory
def inspect_entity_dimensions():
config = parse_arguments()
client = carla.Client(config.host, config.port)
client.set_timeout(5.0)
world = client.get_world()
carla_map = world.get_map()
spawn_point = carla_map.get_spawn_points()[0]
# vehicle blueprints
blueprints = world.get_blueprint_library().filter("vehicle.*")
blueprints = [x for x in blueprints if int(x.get_attribute("number_of_wheels")) == 4]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
blueprints = [x for x in blueprints if not x.id.endswith('firetruck')]
blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')]
blueprints = [x for x in blueprints if not x.id.endswith('ambulance')]
blueprints = [x for x in blueprints if not x.id.endswith('sprinter')]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
# walker blueprints
walker_blueprints = world.get_blueprint_library().filter("walker.*")
vehicle = None
pedestrian = None
spectator = world.get_spectator()
logging = util.AttrDict(
vehicle=util.AttrDict(
xs=[],
d2s=[],
d3s=[]
),
pedestrian=util.AttrDict(
xs=[],
d2s=[],
d3s=[]
)
)
for blueprint in blueprints:
try:
vehicle = world.spawn_actor(blueprint, spawn_point)
world.wait_for_tick()
transform = carlautil.spherical_to_camera_watcher_transform(
5, math.pi, math.pi*(1/6),
pin=spawn_point.location)
spectator.set_transform(transform)
world.wait_for_tick()
x = carlautil.actor_to_bbox_ndarray(vehicle)
d2 = np.linalg.norm(x[:2] / 2)
d3 = np.linalg.norm(x / 2)
logging.vehicle.xs.append(x)
logging.vehicle.d2s.append(d2)
logging.vehicle.d3s.append(d3)
print(blueprint.id)
print(x)
time.sleep(0.1)
finally:
if vehicle:
vehicle.destroy()
vehicle = None
world.wait_for_tick()
for blueprint in walker_blueprints:
try:
pedestrian = world.spawn_actor(blueprint, spawn_point)
world.wait_for_tick()
transform = carlautil.spherical_to_camera_watcher_transform(
5, math.pi, math.pi*(1/6),
pin=spawn_point.location)
spectator.set_transform(transform)
world.wait_for_tick()
x = carlautil.actor_to_bbox_ndarray(pedestrian)
d2 = np.linalg.norm(x[:2] / 2)
d3 = np.linalg.norm(x / 2)
logging.pedestrian.xs.append(x)
logging.pedestrian.d2s.append(d2)
logging.pedestrian.d3s.append(d3)
finally:
if pedestrian:
pedestrian.destroy()
pedestrian = None
for k, entity in logging.items():
entity.xs = np.stack(entity.xs)
entity.d2s = np.stack(entity.d2s)
entity.d3s = np.stack(entity.d3s)
print(f"Vehicle dimensions { np.max(logging.vehicle.xs, 0) }")
print(f" max 2D distance from origin { np.max(logging.vehicle.d2s) }")
print(f" max 3D distance from origin { np.max(logging.vehicle.d3s) }")
print(f"Pedestrian dimensions { np.mean(logging.pedestrian.xs, 0) }")
print(f" max 2D distance from origin { np.max(logging.pedestrian.d2s) }")
print(f" max 3D distance from origin { np.max(logging.pedestrian.d3s) }")
