def make_local_params(self, frame):
"""Get the linearized bicycle model using the vehicle's
immediate orientation."""
params = util.AttrDict()
params.frame = frame
"""Dynamics parameters"""
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle, flip_y=True)
# initial_state - state at current frame in world/local coordinates
# Local coordinates has initial position and heading at 0
initial_state = util.AttrDict(
world=np.array([p_0_x, p_0_y, v_0_x, v_0_y]))
params.initial_state = initial_state
A, B = self.__params.A, self.__params.B
nx, nu = self.__params.nx, self.__params.nu
T = self.__control_horizon
# make state computation account for initial position and velocity
initial_rollout = np.concatenate([
np.linalg.matrix_power(A, t) @ initial_state.world
for t in range(T + 1)
])
params.initial_rollout = util.AttrDict(world=initial_rollout)
return params
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_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(
self.__ego_vehicle, flip_y=True
)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init
)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
"""Get controls for other vehicles."""
# O - number of obstacles
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
return params
def set_goal(self, x=None, y=None, distance=None, is_relative=True, **kwargs):
if x is not None and y is not None:
self.__goal = util.AttrDict(x=x, y=y, is_relative=is_relative)
elif distance is not None:
point = self.__road_boundary.get_point_from_start(distance)
self.__goal = util.AttrDict(x=point[0], y=point[1], is_relative=False)
else:
raise NotImplementedError("Unknown method of setting motion planner goal.")
def __setup_road_boundary_conditions(self, turn_choices, max_distance):
"""Set up a generic road boundary configuration.
TODO: extend RoadBoundaryConstraint so it takes over the role of
__setup_curved_road_segmented_boundary_conditions() and
__setup_rectangular_boundary_conditions()
"""
# __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_boundary : RoadBoundaryConstraint
# Factory for road boundary constraints.
self.__road_boundary = self.__map_reader.road_boundary_constraints_from_actor(
self.__ego_vehicle,
self.__max_distance,
choices=self.__turn_choices,
flip_y=True,
)
n_polytopes = len(self.__road_boundary.road_segs.polytopes)
logging.info(
f"created {n_polytopes} polytopes covering "
f"a distance of {np.round(self.__max_distance, 2)} m in total."
)
x, y = self.__road_boundary.points[-1]
# __goal
# Not used for motion planning when using this BC.
self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def __setup_curved_road_segmented_boundary_conditions(
self, turn_choices, max_distance
):
# __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 : util.AttrDict
# Container of road segment properties.
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
# Not used for motion planning when using this BC.
self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def __init__(self):
"""Load map data from cache and collect shapes of all the intersections."""
self.map_datum = {}
# map to Shapely MultiPolygon for junction entrance/exits
self.map_to_smpolys = {}
# map to Shapely Circle covering junction region
self.map_to_scircles = {}
logger.info("Retrieve map from cache.")
for map_name in CARLA_MAP_NAMES:
cachepath = f"{CACHEDIR}/map_data.{map_name}.pkl"
with open(cachepath, "rb") as f:
payload = dill.load(f, encoding="latin1")
self.map_datum[map_name] = util.AttrDict(
road_polygons=payload["road_polygons"],
white_lines=payload["white_lines"],
yellow_lines=payload["yellow_lines"],
junctions=payload["junctions"],
)
logger.info("Retrieving some data from map.")
for map_name in CARLA_MAP_NAMES:
for _junction in self.map_datum[map_name].junctions:
f = lambda x, y, yaw, l: util.vertices_from_bbox(
np.array([x, y]), yaw, np.array([5.0, 0.95 * l]))
vertex_set = util.map_to_ndarray(
lambda wps: util.starmap(f, wps), _junction.waypoints)
smpolys = util.map_to_list(vertex_set_to_smpoly, vertex_set)
util.setget_list_from_dict(self.map_to_smpolys,
map_name).extend(smpolys)
x, y = _junction.pos
scircle = shapely.geometry.Point(x, y).buffer(
self.TLIGHT_DETECT_RADIUS)
util.setget_list_from_dict(self.map_to_scircles,
map_name).append(scircle)
def extract_junction_points(self, tlight_find_radius=25.):
carla_topology = self.carla_map.get_topology()
junctions = carlautil.get_junctions_from_topology_graph(carla_topology)
tlights = util.filter_to_list(lambda a: 'traffic_light' in a.type_id,
self.carla_world.get_actors())
tlight_distances = np.zeros((
len(tlights),
len(junctions),
))
f = lambda j: carlautil.location_to_ndarray(j.bounding_box.location)
junction_locations = util.map_to_ndarray(f, junctions)
g = lambda tl: carlautil.transform_to_location_ndarray(tl.
get_transform())
tlight_locations = util.map_to_ndarray(g, tlights)
for idx, junction in enumerate(junctions):
tlight_distances[:, idx] = np.linalg.norm(tlight_locations -
junction_locations[idx],
axis=1)
is_controlled_junction = (tlight_distances < tlight_find_radius).any(
axis=0)
is_uncontrolled_junction = np.logical_not(is_controlled_junction)
controlled_junction_locations \
= junction_locations[is_controlled_junction]
uncontrolled_junction_locations \
= junction_locations[is_uncontrolled_junction]
return util.AttrDict(controlled=controlled_junction_locations,
uncontrolled=uncontrolled_junction_locations)
def __init__(
self,
vehicle,
dt=0.03,
break_prop=0.1,
coeff=PIDCoefficients(K_P=1.0, K_I=0.0, K_D=0.0),
):
"""Constructor method.
Parameters
==========
vehicle : carla.Vehicle
Actor to apply to local planner logic onto
dt : float
Time differential in seconds
coeff : PIDCoefficients
Throttle/break PID coefficients.
"""
self.__vehicle = vehicle
self.__coeff = coeff
self.__dt = dt
self.__break_prop = break_prop
self.__clip = util.Clip(low=-1, high=1)
self.__error_buffer = collections.deque(maxlen=8_000)
self.__stats = util.AttrDict(pe=0., ie=0., de=0.)
self.__should_hotfix_mpc = False
def __init__(
self,
vehicle,
max_steering=1.0,
dt=0.03,
coeff=PIDCoefficients(K_P=1.0, K_I=0.0, K_D=0.0),
):
"""Constructor method.
Parameters
==========
vehicle : carla.Vehicle
Actor to apply to local planner logic onto
max_steering : float
The maximum steering angle in radians. For CARLA version 0.9 the Audi A2
can steer a maximum of 57.5 degrees, 1.00 radians (average of two wheels).
dt : float
Time differential in seconds
coeff : PIDCoefficients
PID coefficients.
"""
self.__vehicle = vehicle
self.__max_steering = max_steering
self.__coeff = coeff
self.__dt = dt
self.__clip = util.Clip(low=-1, high=1)
self.__error_buffer = collections.deque(maxlen=8_000)
self.__stats = util.AttrDict(pe=0., ie=0., de=0.)
self.__should_hotfix_mpc = False
def __setup_curved_road_segmented_boundary_conditions(
self, turn_choices, max_distance):
# __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
# Not used for motion planning when using this BC.
self.__goal = util.AttrDict(x=x, y=y, is_relative=False)
def do_prediction(self, frame):
"""Get processed scene object from scene builder, input the scene to a model to
generate the predictions, and then return the predictions and the latents variables.
"""
"""Construct online scene"""
scene = self.__scene_builder.get_scene()
"""Extract Predictions"""
frame_id = int(
(frame - self.__first_frame) / self.__scene_config.record_interval)
timestep = frame_id # we use this as the timestep
timesteps = np.array([timestep])
with torch.no_grad():
z, predictions, nodes, predictions_dict, latent_probs = generate_vehicle_latents(
self.__eval_stg,
scene,
timesteps,
num_samples=self.__n_predictions,
ph=self.__prediction_horizon,
z_mode=False,
gmm_mode=False,
full_dist=False,
all_z_sep=False)
_, past_dict, ground_truth_dict = \
prediction_output_to_trajectories(
predictions_dict, dt=scene.dt, max_h=10,
ph=self.__prediction_horizon, map=None)
return util.AttrDict(scene=scene,
timestep=timestep,
nodes=nodes,
predictions=predictions,
z=z,
latent_probs=latent_probs,
past_dict=past_dict,
ground_truth_dict=ground_truth_dict)
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 __init__(self,
vehicle,
max_steering=1.0,
K_P=1.0,
K_I=0.0,
K_D=0.0,
dt=0.03):
"""Constructor method.
Parameters
==========
vehicle : carla.Vehicle
Actor to apply to local planner logic onto
max_steering : float
The maximum steering angle in radians. For CARLA version 0.9 the Audi A2
can steer a maximum of 57.5 degrees, 1.00 radians (average of two wheels).
K_P : float
Proportional term
K_I : float
Integral term
K_D : float
Differential term
dt : float
Time differential in seconds
"""
self.__vehicle = vehicle
self.__max_steering = max_steering
self.__k_p = K_P
self.__k_i = K_I
self.__k_d = K_D
self.__dt = dt
self.__clip = util.Clip(low=-1, high=1)
self.__error_buffer = collections.deque(maxlen=8_000)
self.__stats = util.AttrDict(pe=0., ie=0., de=0.)
def make_local_params(self, frame, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
"""General local params."""
params.frame = frame
# O - number of obstacles
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle, flip_y=True)
# x0 : np.array
# Initial state
x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
params.x0 = x0
A, T = self.__params.A, self.__params.T
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
"""Local params for (random) multiple coinciding controls."""
# N_traj - number of planned trajectories possible to compute
params.N_traj = np.prod(params.K)
max_K = np.max(params.K)
# TODO: figure out a way to specify number of subtrajectories
params.N_select = min(int(1.5 * max_K), params.N_traj)
# subtraj_indices - the indices of the sub-trajectory to optimize for
params.subtraj_indices = np.random.choice(np.arange(params.N_traj),
size=params.N_select,
replace=False)
return params
def __init__(
self,
ego_vehicle,
map_reader,
n_burn_interval=4,
control_horizon=6,
step_horizon=1,
scene_config=OnlineConfig(),
road_boundary_constraints=True,
#######################
# Logging and debugging
log_cplex=False,
log_agent=False,
plot_simulation=False,
plot_boundary=False,
#######################
# Planned path settings
turn_choices=[],
max_distance=100,
#######################
**kwargs):
self.__ego_vehicle = ego_vehicle
self.__map_reader = map_reader
# __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 timesteps to optimize control over.
self.__control_horizon = control_horizon
# __step_horizon : int
# Number of steps to take at each iteration of MPC.
self.__step_horizon = step_horizon
# __first_frame : int
# First frame in simulation. Used to find current timestep.
self.__first_frame = None
self.__scene_config = scene_config
self.__world = self.__ego_vehicle.get_world()
self.__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.__setup_curved_road_segmented_boundary_conditions(
turn_choices, max_distance)
self.road_boundary_constraints = road_boundary_constraints
self.log_cplex = log_cplex
self.log_agent = log_agent
self.plot_simulation = plot_simulation
self.plot_boundary = plot_boundary
if self.plot_simulation:
self.__plot_simulation_data = util.AttrDict(
actual_trajectory=collections.OrderedDict(),
planned_trajectories=collections.OrderedDict(),
planned_controls=collections.OrderedDict(),
goals=collections.OrderedDict())
def extract_road_polygons_and_lines(self, sampling_precision=0.05):
"""Extract road white and yellow dividing lines, and road polygons.
Parameters
==========
sampling_precision : float
Space between sampling points to create road data.
Returns
=======
util.AttrDict
Payload of road network information with the following keys:
- road_polygons: list of road polygons (closed), each represented as array of shape (?, 2)
- yellow_lines: list of white road lines, each represented as array of shape (?, 2)
- white_lines: list of white road lines, each represented as array of shape (?, 2)
"""
road_polygons = []
yellow_lines = []
white_lines = []
topology = [x[0] for x in self.carla_map.get_topology()]
topology = sorted(topology, key=lambda w: w.transform.location.z)
for waypoint in topology:
waypoints = [waypoint]
nxt = waypoint.next(sampling_precision)[0]
while nxt.road_id == waypoint.road_id:
waypoints.append(nxt)
nxt = nxt.next(sampling_precision)[0]
left_marking = carlautil.locations_to_ndarray([
self.__lateral_shift(w.transform, -w.lane_width * 0.5)
for w in waypoints
],
flip_y=True)
right_marking = carlautil.locations_to_ndarray([
self.__lateral_shift(w.transform, w.lane_width * 0.5)
for w in waypoints
],
flip_y=True)
road_polygon = np.concatenate(
(left_marking, np.flipud(right_marking)), axis=0)
if len(road_polygon) > 2:
road_polygons.append(road_polygon)
if not waypoint.is_intersection:
sample = waypoints[int(len(waypoints) / 2)]
if self.__is_yellow_line(sample, -sample.lane_width * 1.1):
yellow_lines.append(left_marking)
else:
white_lines.append(left_marking)
if self.__is_yellow_line(sample, sample.lane_width * 1.1):
yellow_lines.append(right_marking)
else:
white_lines.append(right_marking)
return util.AttrDict(road_polygons=road_polygons,
yellow_lines=yellow_lines,
white_lines=white_lines)
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_multiple_coinciding_controls(pred_result,
ovehicles,
params,
ctrl_result,
ego_bbox,
filename='lcss_control'):
"""
Parameters
==========
predict_result : util.AttrDict
Payload containing scene, timestep, nodes, predictions, z,
latent_probs, past_dict, ground_truth_dict
ovehicles : list of OVehicle
params : util.AttrDict
ctrl_result : util.AttrDict
ego_bbox : list of int
filename : str
"""
"""Plots for paper"""
for traj_idx, latent_indices in enumerate(
util.product_list_of_list(
[range(ovehicle.n_states) for ovehicle in ovehicles])):
T = params.T
fig, axes = plt.subplots(T // 2 + (T % 2),
2,
figsize=(10, (10 / 4) * T))
axes = axes.ravel()
"""Get scene bitmap"""
scene = pred_result.scene
map_mask = scene.map['VISUALIZATION'].as_image()
map_data = util.AttrDict()
map_data.road_bitmap = np.max(map_mask, axis=2)
map_data.road_div_bitmap = map_mask[..., 1]
map_data.lane_div_bitmap = map_mask[..., 0]
map_data.extent = (scene.x_min, scene.x_max, scene.y_min, scene.y_max)
"""Get control trajectory data"""
X = np.concatenate(
(ctrl_result.start[None], ctrl_result.X_star[traj_idx, :, :2]),
axis=0)
headings = []
for t in range(1, T + 1):
heading = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
headings.append(heading)
headings = np.array(headings)
for t, ax in zip(range(T), axes):
_plot_multiple_coinciding_controls_timestep(
ax, map_data, ovehicles, params, ctrl_result, X, headings, t,
traj_idx, latent_indices, ego_bbox)
fig.tight_layout()
fig.savefig(os.path.join('out', f"{filename}_traj{traj_idx + 1}.png"))
fig.clf()
def test_group_split():
"""Test by manual inspection"""
config = util.AttrDict(n_groups=4)
group_creator = SampleGroupCreator(config)
split_creator = CrossValidationSplitCreator(config)
sample_ids = MOCK_IDS
groups = group_creator.make_groups(sample_ids)
splits = split_creator.make_splits(groups)
split = splits[0]
print()
pp.pprint(groups)
pp.pprint(split)
def make_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
"""Get controls for other vehicles."""
# O - number of total vehicles EV and OVs
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
return params
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 make_local_params(self, frame):
"""Get the local optimization parameters used for current MPC step."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
return params
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 __inspect_node(self, map_name, nodeid):
"""Label the given node by inspecting the node's properties."""
(is_complete_intersection) = self.__inspect_intersection_completedness(
map_name, nodeid)
(is_at_intersection) = self.__inspect_intersectedness(map_name, nodeid)
(is_significant_car,
is_stopped_car) = self.__inspect_distance(map_name, nodeid)
(is_major_turn,
is_minor_turn) = self.__inspect_curvature(map_name, nodeid)
return util.AttrDict(node_id=nodeid,
is_complete_intersection=is_complete_intersection,
is_at_intersection=is_at_intersection,
is_significant_car=is_significant_car,
is_stopped_car=is_stopped_car,
is_major_turn=is_major_turn,
is_minor_turn=is_minor_turn)
def __init__(self,
ego_vehicle,
map_reader,
other_vehicle_ids,
eval_stg,
predict_interval=6,
n_burn_interval=4,
prediction_horizon=8,
n_predictions=100,
scene_config=OnlineConfig()):
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.__predict_interval = predict_interval
self.__n_burn_interval = n_burn_interval
self.__prediction_horizon = prediction_horizon
self.__n_predictions = n_predictions
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.__local_planner = LocalPlanner(self.__ego_vehicle)
self.__goal = util.AttrDict(x=50, y=0, is_relative=True)
def get_current(self):
"""Get reference and measurements after step has been taken step,
and control is applied to vehicle.
Returns
=======
util.AttrDict
Contains the following at current
- measurement
- reference
- control : the last applied control.
"""
control = self.__vehicle.get_control()
control = util.AttrDict(
throttle=control.throttle, brake=control.brake, steer=control.steer
)
error = util.AttrDict(
speed=self.longitudinal_controller.get_current(),
angle=self.lateral_controller.get_current()
)
speed = carlautil.actor_to_speed(self.__vehicle)
_, angle, _ = carlautil.to_rotation_ndarray(self.__vehicle)
if not self.step_to_speed \
or self.__step_idx - 1 >= len(self.step_to_speed):
return util.AttrDict(
measurement=util.AttrDict(speed=speed, angle=angle),
reference=util.AttrDict(speed=speed, angle=angle),
error=error,
control=control
)
reference_speed = self.step_to_speed[self.__step_idx - 1]
reference_angle = self.step_to_angle[self.__step_idx - 1]
return util.AttrDict(
measurement=util.AttrDict(speed=speed, angle=angle),
reference=util.AttrDict(
speed=reference_speed,
angle=reference_angle,
),
error=error,
control=control
)
def make_local_params(self, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
params.O = len(ovehicles) # number of obstacles
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
self.__ego_vehicle)
p_0_y = -p_0_y # need to flip about x-axis
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle)
v_0_y = -v_0_y # need to flip about x-axis
# x0 : np.array
# Initial state
x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
A, T = self.__params.A, self.__params.T
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
return params
def make_local_params(self, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
# O - number of obstacles
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
self.__ego_vehicle, flip_y=True)
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle, flip_y=True)
# x0 : np.array
# Initial state
x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
params.x0 = x0
A, T = self.__params.A, self.__params.T
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
return params
def extract_junction_with_portals(self):
carla_topology = self.carla_map.get_topology()
junctions = carlautil.get_junctions_from_topology_graph(carla_topology)
_junctions = []
for junction in junctions:
jx, jy, _ = carlautil.to_location_ndarray(junction, flip_y=True)
wps = junction.get_waypoints(carla.LaneType.Driving)
_wps = []
for wp1, wp2 in wps:
# wp1 is the waypoint entering into the intersection
# wp2 is the waypoint exiting out of the intersection
x, y, _ = carlautil.to_location_ndarray(wp1, flip_y=True)
_, yaw, _ = carlautil.to_rotation_ndarray(wp1, flip_y=True)
_wp1 = (x, y, yaw, wp1.lane_width)
x, y, _ = carlautil.to_location_ndarray(wp2, flip_y=True)
_, yaw, _ = carlautil.to_rotation_ndarray(wp2, flip_y=True)
_wp2 = (x, y, yaw, wp2.lane_width)
_wps.append((_wp1, _wp2))
_junctions.append(
util.AttrDict(pos=np.array([jx, jy]),
waypoints=np.array(_wps)))
return _junctions
