class DynamicCameraOffset:
def __init__(self):
self.sm = SubMaster(['laneSpeed'])
self.pm = PubMaster(['dynamicCameraOffset'])
self.op_params = opParams()
self.camera_offset = self.op_params.get('camera_offset')
self.left_lane_oncoming = False # these variables change
self.right_lane_oncoming = False
self.last_left_lane_oncoming = False
self.last_right_lane_oncoming = False
self.last_oncoming_time = 0
self.i = 0.0
self._setup_static()
def _setup_static(self): # these variables are static
self._enabled = self.op_params.get('dynamic_camera_offset')
self._min_enable_speed = 35 * CV.MPH_TO_MS
self._min_lane_width_certainty = 0.4
hug = 0.075 # how much to hug
self._center_ratio = 0.5
self._hug_left_ratio = self._center_ratio - hug
self._hug_right_ratio = self._center_ratio + hug
self._keep_offset_for = self.op_params.get(
'dynamic_camera_offset_time') # seconds after losing oncoming lane
self._ramp_angles = [0, 12.5, 25]
self._ramp_angle_mods = [1, 0.85,
0.1] # multiply offset by this based on angle
self._ramp_down_times = [
self._keep_offset_for, self._keep_offset_for + 1.5
]
self._ramp_down_multipliers = [
1, 0
] # ramp down 1.5s after time has passed
self._poly_prob_speeds = [
0, 25 * CV.MPH_TO_MS, 35 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS
]
self._poly_probs = [0.2, 0.25, 0.45,
0.55] # we're good if only one line is above this
self._k_p = 1.5
_i_rate = 1 / 20
self._k_i = 1.2 * _i_rate
def update(self, v_ego, active, angle_steers, lane_width_estimate,
lane_width_certainty, polys, probs):
if self._enabled:
self.sm.update(0)
self.camera_offset = self.op_params.get(
'camera_offset') # update base offset from user
self.left_lane_oncoming = self.sm['laneSpeed'].leftLaneOncoming
self.right_lane_oncoming = self.sm['laneSpeed'].rightLaneOncoming
self.lane_width_estimate, self.lane_width_certainty = lane_width_estimate, lane_width_certainty
self.l_poly, self.r_poly = polys
self.l_prob, self.r_prob = probs
dynamic_offset = self._get_camera_offset(v_ego, active,
angle_steers)
self._send_state(
) # for alerts, before speed check so alerts don't get stuck on
if dynamic_offset is not None:
return self.camera_offset + dynamic_offset
self.i = 0 # reset when not active
return self.camera_offset # don't offset if no lane line in direction we're going to hug
def _get_camera_offset(self, v_ego, active, angle_steers):
self.keeping_left, self.keeping_right = False, False # reset keeping
time_since_oncoming = sec_since_boot() - self.last_oncoming_time
if not active: # no alert when not engaged
return
if np.isnan(self.l_poly[3]) or np.isnan(self.r_poly[3]):
return
if v_ego < self._min_enable_speed:
return
_min_poly_prob = interp(v_ego, self._poly_prob_speeds,
self._poly_probs)
if self.l_prob < _min_poly_prob and self.r_prob < _min_poly_prob: # we only need one line and an accurate current lane width
return
left_lane_oncoming = self.left_lane_oncoming
right_lane_oncoming = self.right_lane_oncoming
if self.have_oncoming:
if self.lane_width_certainty < self._min_lane_width_certainty:
return
self.last_oncoming_time = sec_since_boot()
self.last_left_lane_oncoming = self.left_lane_oncoming # only update last oncoming vars when currently have oncoming. one should always be True for the 2 second ramp down
self.last_right_lane_oncoming = self.right_lane_oncoming
elif time_since_oncoming > self._keep_offset_for: # return if it's 2+ seconds after last oncoming, no need to offset
return
else: # no oncoming and not yet 2 seconds after we lost an oncoming lane. use last oncoming lane for 2 seconds to ramp down offset
left_lane_oncoming = self.last_left_lane_oncoming
right_lane_oncoming = self.last_right_lane_oncoming
estimated_lane_position = self._get_camera_position()
hug_modifier = interp(
abs(angle_steers), self._ramp_angles,
self._ramp_angle_mods) # don't offset as much when angle is high
if left_lane_oncoming:
self.keeping_right = True
hug_ratio = (self._hug_right_ratio * hug_modifier) + (
self._center_ratio * (1 - hug_modifier)) # weighted average
elif right_lane_oncoming:
self.keeping_left = True
hug_ratio = (self._hug_left_ratio *
hug_modifier) + (self._center_ratio *
(1 - hug_modifier))
else:
raise Exception('Error, no lane is oncoming but we\'re here!')
error = estimated_lane_position - hug_ratio
self.i += error * self._k_i # PI controller
offset = self.i + error * self._k_p
if time_since_oncoming <= self._keep_offset_for and not self.have_oncoming: # not yet 3 seconds after last oncoming, ramp down from 1.5 second
offset *= interp(time_since_oncoming, self._ramp_down_times,
self._ramp_down_multipliers) # ramp down offset
return offset
def _send_state(self):
dco_send = messaging.new_message('dynamicCameraOffset')
dco_send.dynamicCameraOffset.keepingLeft = self.keeping_left
dco_send.dynamicCameraOffset.keepingRight = self.keeping_right
self.pm.send('dynamicCameraOffset', dco_send)
@property
def have_oncoming(self):
return self.left_lane_oncoming != self.right_lane_oncoming # only one lane oncoming
def _get_camera_position(self):
"""
Returns the position of the camera in the lane as a percentage. left to right: [0, 1]; 0.5 is centered
You MUST verify that either left or right polys and lane width are accurate before calling this function.
"""
left_line_pos = self.l_poly[
3] + self.camera_offset # polys have not been offset yet
right_line_pos = self.r_poly[3] + self.camera_offset
cam_pos_left = left_line_pos / self.lane_width_estimate # estimated position of car in lane based on left line
cam_pos_right = 1 - abs(
right_line_pos
) / self.lane_width_estimate # estimated position of car in lane based on right line
# find car's camera position using weighted average of lane poly certainty
# if certainty of both lines are high, then just average ~equally
l_prob = self.l_prob / (self.l_prob + self.r_prob
) # this and next line sums to 1
r_prob = self.r_prob / (self.l_prob + self.r_prob)
# be biased towards position found from most probable lane line
return cam_pos_left * l_prob + cam_pos_right * r_prob
0.47851562, 0.625, 0.79101562, 0.9765625, 1.1816406, 1.40625, 1.6503906,
1.9140625, 2.1972656, 2.5, 2.8222656, 3.1640625, 3.5253906, 3.90625,
4.3066406, 4.7265625, 5.1660156, 5.625, 6.1035156, 6.6015625, 7.1191406,
7.65625, 8.2128906, 8.7890625, 9.3847656, 10
]
mpc_idxs = list(range(10))
model_t_idx = [
sorted(range(len(model_t)),
key=[abs(idx - t) for t in model_t].__getitem__)[0]
for idx in mpc_idxs
] # matches 0 to 9 interval to idx from t
# speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0] # idx used for current speed, position still uses model_t_idx
while 1:
sm.update()
modelV2 = sm['modelV2']
if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
continue
distances, speeds, accelerations = [], [], [
] # everything is derived from x position since velocity is outputting weird values
for t in model_t_idx:
speeds.append(modelV2.velocity.x[t])
distances.append(modelV2.position.x[t])
if model_t_idx.index(
t
) > 0: # skip first since we can't calculate (and don't want to use v_ego)
accelerations.append((speeds[-1] - speeds[-2]) / model_t[t])