def step(self, target_angle):
"""Execute one step of lateral control reach a certain target vehicle angle.
NOTE: target and measurements are of vehicle longitudinal angle.
Parameters
==========
target_angle : float
Target vehicle longitudinal angle in radians.
Returns
=======
float
Steering control in the range [-1, 1] where
-1 maximum steering to left and
+1 maximum steering to right
"""
_, current_angle, _ = carlautil.to_rotation_ndarray(self.__vehicle)
current_angle = util.npu.warp_radians_about_center(current_angle, target_angle)
error = target_angle - current_angle
self.__error_buffer.append(error)
if len(self.__error_buffer) >= 2:
_ie = sum(self.__error_buffer) * self.__dt
else:
_ie = 0.0
if self.__should_hotfix_mpc:
self.__should_hotfix_mpc = False
_de = 0.0
elif len(self.__error_buffer) >= 2:
_de = (self.__error_buffer[-1] - self.__error_buffer[-2]) / self.__dt
else:
_de = 0.0
self.__store_current(error, _ie, _de)
ctrl_input = (self.__coeff.K_P * error) + (self.__coeff.K_D * _de) + (self.__coeff.K_I * _ie)
ctrl_input = ctrl_input / self.__max_steering
return self.__clip(ctrl_input)
def add_stats():
# save the speed, heading and control values
speed = carlautil.actor_to_speed(ego_vehicle)
_, heading, _ = carlautil.to_rotation_ndarray(ego_vehicle)
speeds.append(speed)
headings.append(heading)
control = ego_vehicle.get_control()
control_steers.append(control.steer)
control_throttles.append(control.throttle)
control_brakes.append(control.brake)
def set_plan(self, target_speeds, target_angles, step_period):
"""Given a trajectory consisting of heading angle and
velocities, use lateral and longitudinal PID controllers
to actuate the vehicle.
Parameters
==========
target_speeds : iterable of float
Target speeds in m/s for the next few consecutive steps.
target_angles : iterable of float
Target angles in radians for the next few consecutive steps.
Angles should be in radians and in the Unreal Engine coordinate system.
The iterables `target_speed` and `target_angle` should have the same length.
step_period : int
The fixed number of steps between two consecutive points in the trajectory.
Each step takes `carla.WorldSettings.fixed_delta_seconds` time.
"""
speed = carlautil.actor_to_speed(self.__vehicle)
_, heading, _ = carlautil.to_rotation_ndarray(self.__vehicle)
target_speeds = np.concatenate(([speed], target_speeds))
target_angles = np.concatenate(([heading], target_angles))
# angles are not expected to lie within (-pi, pi]. Enforce this constraint.
target_angles = util.npu.warp_radians_neg_pi_to_pi(target_angles)
self.step_to_speed = []
self.step_to_angle = []
n_steps = len(target_speeds) - 1
for step in range(n_steps):
nxt1 = target_angles[step+1]
nxt2 = target_angles[step+1] + 2*np.pi
nxt3 = target_angles[step+1] - 2*np.pi
dif1 = abs(target_angles[step] - nxt1)
dif2 = abs(target_angles[step] - nxt2)
dif3 = abs(target_angles[step] - nxt3)
if dif1 < dif2 and dif1 < dif3:
nxt = nxt1
elif dif2 < dif1 and dif2 < dif3:
nxt = nxt2
else:
nxt = nxt3
for substep in range(step_period):
self.step_to_speed.append(
target_speeds[step] + (substep/step_period)*(target_speeds[step+1] - target_speeds[step])
)
self.step_to_angle.append(
util.npu.warp_radians_neg_pi_to_pi(
target_angles[step] + (substep/step_period)*(nxt - target_angles[step])
)
)
self.step_to_speed.append(target_speeds[-1])
self.step_to_angle.append(target_angles[-1])
self.__step_idx = 1
self.longitudinal_controller.hotfix_mpc()
self.lateral_controller.hotfix_mpc()
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
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 get_straight_lanes(start_wp, tol=2.0):
"""Get the points corresponding to the straight parts of all
lanes in road starting from start_wp going in the same direction.
Parameters
==========
start_wp : carla.Waypoint
The starting point of the road lane.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
list of np.array
Each item in the list are points for one lane of of shape (N_i, 2).
Each (x, y) point is in meters in world coordinates.
"""
_, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp)
wps = get_adjacent_waypoints(start_wp)
f = lambda wp: get_straight_line(wp, start_yaw, tol=tol)
return np.concatenate(util.map_to_list(f, wps))
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