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])