def __compute_prediction_controls(self, frame):
params = self.make_local_params(frame)
ctrl_result = self.do_highlevel_control(params)
"""use control input next round for warm starting."""
# self.U_warmstarting = ctrl_result.U_star
"""Get trajectory and velocity"""
trajectory = []
velocity = []
X = np.concatenate(
(params.initial_state.world[None], ctrl_result.X_star))
n_steps = X.shape[0]
for t in range(1, n_steps):
x, y, yaw = X[t, :3]
y = -y # flip about x-axis again to move back to UE coordinates
# 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])
yaw = np.rad2deg(util.reflect_radians_about_x_axis(yaw))
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
velocity.append(X[t, 3])
if self.plot_simulation:
"""Save planned trajectory for final plotting"""
self.__plot_simulation_data.planned_trajectories[frame] = X
self.__plot_simulation_data.planned_controls[
frame] = ctrl_result.U_star
self.__plot_simulation_data.goals[frame] = ctrl_result.goal
return trajectory, velocity
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 compute_acceleration_constraints(self, u_x, u_y):
"""Accelaration control inputs have coupled constraints.
Generate docplex constraints for velocity for double integrators.
Present performance cars are capable of going from 0 to 60 mph in under 5 seconds.
Reference:
https://en.wikipedia.org/wiki/0_to_60_mph
Parameters
==========
u_x : np.array of docplex.mp.vartype.VarType
u_y : np.array of docplex.mp.vartype.VarType
"""
_, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle)
theta = util.reflect_radians_about_x_axis(theta)
r = -2.5
a_1 = 7.5
a_2 = 5.0
c1 = a_2*((u_x - r*np.cos(theta))*np.cos(theta) \
+ (u_y - r*np.sin(theta))*np.sin(theta))
c2 = a_1*((u_y - r*np.sin(theta))*np.cos(theta) \
- (u_x - r*np.cos(theta))*np.sin(theta))
c3 = np.abs(a_1 * a_2)
constraints = []
constraints.extend([z <= c3 for z in c1 + c2])
constraints.extend([z <= c3 for z in -c1 + c2])
constraints.extend([z <= c3 for z in c1 - c2])
constraints.extend([z <= c3 for z in -c1 - c2])
return constraints
def compute_velocity_constraints(self, v_x, v_y):
"""Velocity states have coupled constraints.
Generate docplex constraints for velocity for double integrators.
Street speed limit is 30 km/h == 8.33.. m/s
Parameters
==========
v_x : np.array of docplex.mp.vartype.VarType
v_y : np.array of docplex.mp.vartype.VarType
"""
v_lim = self.__ego_vehicle.get_speed_limit() # is m/s
_, theta, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle)
theta = util.reflect_radians_about_x_axis(theta)
r = v_lim / 2
v_1 = r
v_2 = 0.75 * v_lim
c1 = v_2*((v_x - r*np.cos(theta))*np.cos(theta) \
+ (v_y - r*np.sin(theta))*np.sin(theta))
c2 = v_1*((v_y - r*np.sin(theta))*np.cos(theta) \
- (v_x - r*np.cos(theta))*np.sin(theta))
c3 = np.abs(v_1 * v_2)
constraints = []
constraints.extend([z <= c3 for z in c1 + c2])
constraints.extend([z <= c3 for z in -c1 + c2])
constraints.extend([z <= c3 for z in c1 - c2])
constraints.extend([z <= c3 for z in -c1 - c2])
return constraints
def __build_scene_data(self):
"""Does post/mid collection processing of the data.
This method copies the data and does processing on the copy if
__scene_config is a SceneConfig, or directly processes the data if
__scene_config is a OnlineConfig.
Reflects all coordinates data about the x-axis in-place.
This is needed because CARLA 0.9.11 uses a flipped y-axis.
Returns
=======
np.array
Current overhead points collected by scene builder of shape (# points, 3)
that has been flipped about the x-axis.
pd.DataFrame
Current trajectory data collected by scene builder with relevant data
(position, velocity, acceleration, heading) flipped about the x-axis.
"""
scene_data = SceneBuilderData(
self.__save_directory,
self.__scene_name,
self.__map_reader.map_name,
self.__world.get_settings().fixed_delta_seconds,
self.__trajectory_data,
self.__overhead_points,
self.__overhead_labels,
self.__overhead_ids,
self.__map_reader.map_data,
self.__vehicle_visibility,
self.__sensor_loc_at_t0,
self.__first_frame,
self.__scene_config,
)
if self.is_online:
"""Copies the scene data.
WARNING: map_data, scene_config are passed as reference."""
attr_names = [
"trajectory_data",
"overhead_points",
"overhead_labels",
"overhead_ids",
"sensor_loc_at_t0",
"vehicle_visibility",
]
for name in attr_names:
setattr(scene_data, name,
copy.deepcopy(getattr(scene_data, name)))
"""Reflect and return scene raw data."""
scene_data.overhead_points[:, 1] *= -1
scene_data.trajectory_data[["y", "v_y", "a_y"]] *= -1
scene_data.trajectory_data[
"heading"] = util.reflect_radians_about_x_axis(
scene_data.trajectory_data["heading"])
"""Sort Trajectory data"""
scene_data.trajectory_data.sort_values("frame_id", inplace=True)
return scene_data
def __setup_rectangular_boundary_conditions(self):
# __road_segment_enclosure : np.array
# Array of shape (4, 2) enclosing the road segment
# __road_seg_starting : np.array
# The position and the heading angle of the starting waypoint
# of the road of form [x, y, angle] in (meters, meters, radians).
self.__road_seg_starting, self.__road_seg_enclosure, self.__road_seg_params \
= self.__map_reader.road_segment_enclosure_from_actor(self.__ego_vehicle)
self.__road_seg_starting[1] *= -1 # need to flip about x-axis
self.__road_seg_starting[2] = util.reflect_radians_about_x_axis(
self.__road_seg_starting[2]) # need to flip about x-axis
self.__road_seg_enclosure[:, 1] *= -1 # need to flip about x-axis
# __goal
# Goal destination the vehicle should navigates to.
self.__goal = util.AttrDict(x=50, y=0, is_relative=True)
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 __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 __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_local_params(frame, 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 = []
velocity = []
X = np.concatenate((params.x0[None], ctrl_result.X_star))
n_steps = X.shape[0]
headings = []
for t in range(1, n_steps):
x, y = X[t, :2]
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 = np.rad2deg(util.reflect_radians_about_x_axis(yaw))
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
velocity.append(math.sqrt(X[t, 2]**2 + X[t, 3]**2))
if self.plot_scenario:
"""Plot scenario"""
ctrl_result.headings = headings
self.__plot_scenario(pred_result, ovehicles, params, ctrl_result)
if self.plot_simulation:
"""Save planned trajectory for final plotting"""
if self.__agent_type == "oa":
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
else:
raise NotImplementedError()
return trajectory, velocity
def __init__(self,
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
control_horizon=6,
n_burn_interval=4,
prediction_horizon=8,
n_predictions=100,
n_coincide=6,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
scene_config=OnlineConfig(),
log_cplex=False,
log_agent=False,
plot_scenario=False,
plot_boundary=False,
plot_vertices=False,
plot_overapprox=False,
**kwargs):
assert n_coincide <= prediction_horizon
assert control_horizon <= n_coincide
self.__ego_vehicle = ego_vehicle
self.__map_reader = map_reader
self.__world = self.__ego_vehicle.get_world()
# __first_frame : int
# First frame in simulation. Used to find current timestep.
self.__first_frame = None
self.__scene_builder = None
self.__scene_config = scene_config
self.__scene_builder_cls = scene_builder_cls
# __control_horizon : int
# Number of predictions timesteps to conduct control over.
self.__control_horizon = control_horizon
# __n_burn_interval : int
# Interval in prediction timesteps to skip prediction and control.
self.__n_burn_interval = n_burn_interval
# __prediction_horizon : int
# Number of predictions timesteps to predict other vehicles over.
self.__prediction_horizon = prediction_horizon
self.__n_predictions = n_predictions
self.__n_coincide = n_coincide
self.__eval_stg = eval_stg
vehicles = self.__world.get_actors(other_vehicle_ids)
# __other_vehicles : list of carla.Vehicle
# List of IDs of vehicles not including __ego_vehicle.
# Use this to track other vehicles in the scene at each timestep.
self.__other_vehicles = dict(zip(other_vehicle_ids, vehicles))
# __sensor : carla.Sensor
# Segmentation sensor. Data points will be used to construct overhead.
self.__sensor = self.__create_segmentation_lidar_sensor()
# __lidar_feeds : collections.OrderedDict
# Where int key is frame index and value
# is a carla.LidarMeasurement or carla.SemanticLidarMeasurement
self.__lidar_feeds = collections.OrderedDict()
self.__prediction_timestep = self.__scene_config.record_interval \
* self.__world.get_settings().fixed_delta_seconds
self.__local_planner = LocalPlanner(self.__ego_vehicle)
self.__params = self.__make_global_params()
self.__goal = util.AttrDict(x=50, y=0, is_relative=True)
# __road_segment_enclosure : np.array
# Array of shape (4, 2) enclosing the road segment
# __road_seg_starting : np.array
# The position and the heading angle of the starting waypoint
# of the road of form [x, y, angle] in (meters, meters, radians).
self.__road_seg_starting, self.__road_seg_enclosure, self.__road_seg_params \
= self.__map_reader.road_segment_enclosure_from_actor(self.__ego_vehicle)
self.__road_seg_starting[1] *= -1 # need to flip about x-axis
self.__road_seg_starting[2] = util.reflect_radians_about_x_axis(
self.__road_seg_starting[2]) # need to flip about x-axis
self.__road_seg_enclosure[:, 1] *= -1 # need to flip about x-axis
self.log_cplex = log_cplex
self.log_agent = log_agent
self.plot_scenario = plot_scenario
self.plot_boundary = plot_boundary
self.plot_vertices = plot_vertices
self.plot_overapprox = plot_overapprox
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_local_params(frame, ovehicles)
if self.__agent_type == "oa":
ctrl_result = self.do_single_control(params, ovehicles)
elif self.__agent_type == "mcc" or self.__agent_type == "rmcc":
ctrl_result = self.do_multiple_control(params, ovehicles)
else:
raise NotImplementedError()
"""use control input next round for warm starting."""
# self.U_warmstarting = ctrl_result.U_star
"""Get trajectory"""
trajectory = []
velocity = []
# X has shape (T+1, nx)
if self.__agent_type == "oa":
X = np.concatenate((params.x0[None], ctrl_result.X_star))
n_steps = X.shape[0]
elif self.__agent_type == "mcc" or self.__agent_type == "rmcc":
X = np.concatenate((params.x0[None], ctrl_result.X_star[0]))
n_steps = self.__n_coincide + 1
if self.log_agent:
logging.info(f"Optimized {params.N_traj} "
f"trajectories avoiding {params.O} vehicles.")
else:
raise NotImplementedError()
headings = []
for t in range(1, n_steps):
x, y = X[t, :2]
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 = np.rad2deg(util.reflect_radians_about_x_axis(yaw))
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
velocity.append(math.sqrt(X[t, 2]**2 + X[t, 3]**2))
if self.plot_scenario:
"""Plot scenario"""
ctrl_result.headings = headings
self.__plot_scenario(pred_result, ovehicles, params, ctrl_result)
if self.plot_simulation:
"""Save planned trajectory for final plotting"""
if self.__agent_type == "oa":
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
elif self.__agent_type == "mcc" or self.__agent_type == "rmcc":
N_select = params.N_select if self.__agent_type == "rmcc" else params.N_traj
self.__plot_simulation_data.planned_trajectories[
frame] = np.concatenate(
(np.repeat(params.x0[None], N_select,
axis=0)[:, None], ctrl_result.X_star),
axis=1)
self.__plot_simulation_data.planned_controls[
frame] = ctrl_result.U_star
else:
pass
return trajectory, velocity
def __init__(
self,
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
n_burn_interval=4,
control_horizon=6,
prediction_horizon=8,
n_predictions=100,
scene_builder_cls=TrajectronPlusPlusSceneBuilder,
scene_config=OnlineConfig(),
#########################
# Controller type setting
agent_type="oa",
n_coincide=6,
#######################
# Logging and debugging
log_cplex=False,
log_agent=False,
plot_scenario=False,
plot_simulation=False,
plot_boundary=False,
plot_vertices=False,
plot_overapprox=False,
#######################
# Planned path settings
turn_choices=[],
max_distance=100,
#######################
**kwargs):
self.__ego_vehicle = ego_vehicle
self.__map_reader = map_reader
self.__eval_stg = eval_stg
# __n_burn_interval : int
# Interval in prediction timesteps to skip prediction and control.
self.__n_burn_interval = n_burn_interval
# __control_horizon : int
# Number of predictions timesteps to conduct control over.
self.__control_horizon = control_horizon
# __prediction_horizon : int
# Number of predictions timesteps to predict other vehicles over.
self.__prediction_horizon = prediction_horizon
# __n_predictions : int
# Number of predictions to generate on each control step.
self.__n_predictions = n_predictions
self.__scene_builder_cls = scene_builder_cls
self.__scene_config = scene_config
if agent_type == "oa":
assert control_horizon <= prediction_horizon
elif agent_type == "mcc" or agent_type == "rmcc":
assert n_coincide <= prediction_horizon
assert control_horizon <= n_coincide
else:
raise ValueError(f"Agent type {agent_type} is not recognized.")
self.__agent_type = agent_type
# __n_coincide : int
# Number of steps in motion plan that coincide.
# Used for multiple coinciding control.
self.__n_coincide = n_coincide
# __first_frame : int
# First frame in simulation. Used to find current timestep.
self.__first_frame = None
self.__scene_builder = None
self.__world = self.__ego_vehicle.get_world()
vehicles = self.__world.get_actors(other_vehicle_ids)
# __other_vehicles : list of carla.Vehicle
# List of IDs of vehicles not including __ego_vehicle.
# Use this to track other vehicles in the scene at each timestep.
self.__other_vehicles = dict(zip(other_vehicle_ids, vehicles))
# __sensor : carla.Sensor
# Segmentation sensor. Data points will be used to construct overhead.
self.__sensor = self.__create_segmentation_lidar_sensor()
# __lidar_feeds : collections.OrderedDict
# Where int key is frame index and value
# is a carla.LidarMeasurement or carla.SemanticLidarMeasurement
self.__lidar_feeds = collections.OrderedDict()
self.__prediction_timestep = self.__scene_config.record_interval \
* self.__world.get_settings().fixed_delta_seconds
self.__local_planner = LocalPlanner(self.__ego_vehicle)
self.__params = self.__make_global_params()
####################################################
# rectangular boundary constraints are not used here
####################################################
# __road_segment_enclosure : np.array
# Array of shape (4, 2) enclosing the road segment
# __road_seg_starting : np.array
# The position and the heading angle of the starting waypoint
# of the road of form [x, y, angle] in (meters, meters, radians).
self.__road_seg_starting, self.__road_seg_enclosure, self.__road_seg_params \
= self.__map_reader.road_segment_enclosure_from_actor(self.__ego_vehicle)
self.__road_seg_starting[1] *= -1 # need to flip about x-axis
self.__road_seg_starting[2] = util.reflect_radians_about_x_axis(
self.__road_seg_starting[2]) # need to flip about x-axis
self.__road_seg_enclosure[:, 1] *= -1 # need to flip about x-axis
############################################
# curved road segmented boundary constraints
############################################
# __turn_choices : list of int
# List of choices of turns to make at intersections,
# starting with the first intersection to the last.
self.__turn_choices = turn_choices
# __max_distance : number
# Maximum distance from road
self.__max_distance = max_distance
# __road_segs.spline : scipy.interpolate.CubicSpline
# The spline representing the path the vehicle should motion plan on.
# __road_segs.polytopes : list of (ndarray, ndarray)
# List of polytopes in H-representation (A, b) where x is in polytope if Ax <= b.
# __road_segs.distances : ndarray
# The distances along the spline to follow from nearest endpoint
# before encountering corresponding covering polytope in index.
# __road_segs.positions : ndarray
# The 2D positions of center of the covering polytope in index.
self.__road_segs = self.__map_reader.curved_road_segments_enclosure_from_actor(
self.__ego_vehicle,
self.__max_distance,
choices=self.__turn_choices,
flip_y=True)
logging.info(
f"max curvature of planned path is {self.__road_segs.max_k}; "
f"created {len(self.__road_segs.polytopes)} polytopes covering "
f"a distance of {np.round(self.__max_distance, 2)} m in total.")
x, y = self.__road_segs.spline(self.__road_segs.distances[-1])
# __goal
# Goal destination the vehicle should navigates to.
# Not used for motion planning.
self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
self.log_cplex = log_cplex
self.log_agent = log_agent
self.plot_scenario = plot_scenario
self.plot_simulation = plot_simulation
self.plot_boundary = plot_boundary
self.plot_vertices = plot_vertices
self.plot_overapprox = plot_overapprox
if self.plot_simulation:
self.__plot_simulation_data = util.AttrDict(
actual_trajectory=collections.OrderedDict(),
planned_trajectories=collections.OrderedDict(),
planned_controls=collections.OrderedDict())
