def compute_boundary_constraints(self, p_x, p_y):
"""
Parameters
==========
p_x : np.array of docplex.mp.vartype.VarType
Has p_x = <p_x_1, p_x_2, ..., p_x_T> where timesteps T is the
extent of motion plan to enforce constraints in the x axis.
p_y : np.array of docplex.mp.vartype.VarType
Has p_y = <p_y_1, p_y_2, ..., p_y_T> where timesteps T is the
extent of motion plan to enforce constraints in the y axis.
"""
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
if self.plot_boundary:
self.__plot_boundary()
pos = np.stack([p_x, p_y], axis=1)
pos = util.obj_matmul((pos - shift), mtx.T)
constraints = []
constraints.extend([-b_length <= z for z in pos[:, 0]])
constraints.extend([z <= f_length for z in pos[:, 0]])
constraints.extend([-r_width <= z for z in pos[:, 1]])
constraints.extend([z <= l_width for z in pos[:, 1]])
return constraints
def compute_boundary_constraints(self, p_x, p_y):
"""
Parameters
==========
p_x : np.array of docplex.mp.vartype.VarType
p_y : np.array of docplex.mp.vartype.VarType
"""
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
plot_bbox_constraints = False
if plot_bbox_constraints:
fig, axes = plt.subplots(1, 2, figsize=(10, 6))
axes = axes.ravel()
ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle)
ego_y = -ego_y
wp_x, wp_y = self.__road_seg_starting[:2]
axes[0].plot(ego_x, ego_y, 'g*')
axes[0].plot(wp_x, wp_y, 'b*')
patch = patches.Polygon(self.__road_seg_enclosure,
ec='b',
fc='none')
axes[0].add_patch(patch)
axes[0].set_aspect('equal')
ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift)
axes[1].plot(ego_x, ego_y, 'g*')
axes[1].plot([0, -b_length], [0, 0], '-bo')
axes[1].plot([0, f_length], [0, 0], '-bo')
axes[1].plot([0, 0], [0, -r_width], '-bo')
axes[1].plot([0, 0], [0, l_width], '-bo')
axes[1].set_aspect('equal')
fig.tight_layout()
plt.show()
pos = np.stack([p_x, p_y], axis=1)
pos = obj_matmul((pos - shift), mtx.T)
constraints = []
constraints.extend([-b_length <= z for z in pos[:, 0]])
constraints.extend([z <= f_length for z in pos[:, 0]])
constraints.extend([-r_width <= z for z in pos[:, 1]])
constraints.extend([z <= l_width for z in pos[:, 1]])
return constraints
def compute_boundary_constraints(self, p_x, p_y):
"""
Parameters
==========
p_x : np.array of docplex.mp.vartype.VarType
p_y : np.array of docplex.mp.vartype.VarType
"""
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
if self.plot_boundary:
self.__plot_boundary()
pos = np.stack([p_x, p_y], axis=1)
pos = util.obj_matmul((pos - shift), mtx.T)
constraints = []
constraints.extend([-b_length <= z for z in pos[:, 0]])
constraints.extend([z <= f_length for z in pos[:, 0]])
constraints.extend([-r_width <= z for z in pos[:, 1]])
constraints.extend([z <= l_width for z in pos[:, 1]])
return constraints
def __plot_boundary(self):
fig, axes = plt.subplots(1, 2, figsize=(10, 6))
axes = axes.ravel()
ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle)
ego_y = -ego_y
wp_x, wp_y = self.__road_seg_starting[:2]
axes[0].plot(ego_x, ego_y, 'g*')
axes[0].plot(wp_x, wp_y, 'b*')
patch = patches.Polygon(self.__road_seg_enclosure, ec='b', fc='none')
axes[0].add_patch(patch)
axes[0].set_aspect('equal')
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift)
axes[1].plot(ego_x, ego_y, 'g*')
axes[1].plot([0, -b_length], [0, 0], '-bo')
axes[1].plot([0, f_length], [0, 0], '-bo')
axes[1].plot([0, 0], [0, -r_width], '-bo')
axes[1].plot([0, 0], [0, l_width], '-bo')
axes[1].set_aspect('equal')
fig.tight_layout()
plt.show()
def get_road_segment_enclosure(start_wp, tol=2.0):
"""Get rectangle that tightly inner approximates of the road segment
containing the starting waypoint.
Parameters
==========
start_wp : carla.Waypoint
A starting waypoint of the road.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
np.array
The position and the heading angle of the starting waypoint
of the road of form [x, y, angle] in (meters, meters, radians).
np.array
The 2D bounding box enclosure in world coordinates of shape (4, 2)
enclosing the road segment.
np.array
The parameters of the enclosure of form
(b_length, f_length, r_width, l_width)
If the enclosure is in the reference frame such that the starting
waypoint points along +x-axis, then the enclosure has these length
and widths:
____________________________________
| l_width |
| | |
| | |
| b_length -- (x, y)-> -- f_length |
| | |
| | |
| r_width |
------------------------------------
"""
_LENGTH = -2
_, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp)
adj_wps = get_adjacent_waypoints(start_wp)
# mtx : np.array
# Rotation matrix from world coordinates, both frames in UE orientation
mtx = util.rotation_2d(-start_yaw)
# rev_mtx : np.array
# Rotation matrix to world coordinates, both frames in UE orientation
rev_mtx = util.rotation_2d(start_yaw)
s_x, s_y, _ = carlautil.to_location_ndarray(start_wp)
# Get points of lanes
f = lambda wp: get_straight_line(wp, start_yaw, tol=tol)
pc = util.map_to_list(f, adj_wps)
# Get length of bb for lanes
def g(points):
points = points - np.array([s_x, s_y])
points = (rev_mtx @ points.T)[0]
return np.abs(np.max(points) - np.min(points))
lane_lengths = util.map_to_ndarray(g, pc)
length = np.min(lane_lengths)
# Get width of bb for lanes
lwp, rwp = adj_wps[0], adj_wps[-1]
l_x, l_y, _ = carlautil.to_location_ndarray(lwp)
r_x, r_y, _ = carlautil.to_location_ndarray(rwp)
points = np.array([[l_x, l_y], [s_x, s_y], [r_x, r_y]])
points = points @ rev_mtx.T
l_width = np.abs(points[0, 1] - points[1, 1]) + lwp.lane_width / 2.
r_width = np.abs(points[1, 1] - points[2, 1]) + rwp.lane_width / 2.
# construct bounding box of road segment
x, y, _ = carlautil.to_location_ndarray(start_wp)
vec = np.array([[0, 0], [_LENGTH, 0]]) @ mtx.T
dx0, dy0 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [length, 0]]) @ mtx.T
dx1, dy1 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, -l_width]]) @ mtx.T
dx2, dy2 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, r_width]]) @ mtx.T
dx3, dy3 = vec[1, 0], vec[1, 1]
bbox = np.array([[x + dx0 + dx3, y + dy0 + dy3],
[x + dx1 + dx3, y + dy1 + dy3],
[x + dx1 + dx2, y + dy1 + dy2],
[x + dx0 + dx2, y + dy0 + dy2]])
start_wp_spec = np.array([s_x, s_y, start_yaw])
bbox_spec = np.array([_LENGTH, length, r_width, l_width])
return start_wp_spec, bbox, bbox_spec