def update_d_poly(self, v_ego):
# only offset left and right lane lines; offsetting p_poly does not make sense
cameraOffset = ntune_get("cameraOffset")
self.l_poly[3] += cameraOffset
self.r_poly[3] += cameraOffset
# Reduce reliance on lanelines that are too far apart or
# will be in a few seconds
l_prob, r_prob = self.l_prob, self.r_prob
width_poly = self.l_poly - self.r_poly
prob_mods = []
for t_check in [0.0, 1.5, 3.0]:
width_at_t = eval_poly(width_poly, t_check * (v_ego + 7))
prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
mod = min(prob_mods)
l_prob *= mod
r_prob *= mod
# Reduce reliance on uncertain lanelines
l_std_mod = interp(self.l_std, [.15, .3], [1.0, 0.0])
r_std_mod = interp(self.r_std, [.15, .3], [1.0, 0.0])
l_prob *= l_std_mod
r_prob *= r_std_mod
# Find current lanewidth
self.lane_width_certainty += 0.05 * (l_prob * r_prob -
self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.l_poly.copy()
path_from_left_lane[3] -= clipped_lane_width / 2.0
path_from_right_lane = self.r_poly.copy()
path_from_right_lane[3] += clipped_lane_width / 2.0
lr_prob = l_prob + r_prob - l_prob * r_prob
# neokii
# if lr_prob > 0.65:
# lr_prob = min(lr_prob * 1.35, 1.0)
# neokii/stonerains
if lr_prob > 0.65:
lr_prob = min(lr_prob * 1.35, 1.0)
elif lr_prob > 0.32:
lr_prob = min(lr_prob * 1.625, 0.93)
d_poly_lane = (l_prob * path_from_left_lane + r_prob *
path_from_right_lane) / (l_prob + r_prob + 0.0001)
self.d_poly = lr_prob * d_poly_lane + (1.0 -
lr_prob) * self.p_poly.copy()
def setup_mpc(self):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING,
ntune_get('steerRateCost'))
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].curvature = 0.0
self.desired_curvature = 0.0
self.safe_desired_curvature = 0.0
self.desired_curvature_rate = 0.0
self.safe_desired_curvature_rate = 0.0
def parse_model(self, md):
if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
# left and right ll x is the same
self.ll_x = md.laneLines[1].x
# only offset left and right lane lines; offsetting path does not make sense
cameraOffset = ntune_get("cameraOffset")
self.lll_y = np.array(md.laneLines[1].y) - cameraOffset
self.rll_y = np.array(md.laneLines[2].y) - cameraOffset
self.lll_prob = md.laneLineProbs[1]
self.rll_prob = md.laneLineProbs[2]
self.lll_std = md.laneLineStds[1]
self.rll_std = md.laneLineStds[2]
if len(md.meta.desireState):
self.l_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeLeft]
self.r_lane_change_prob = md.meta.desireState[log.LateralPlan.Desire.laneChangeRight]
def __init__(self, CP):
self.LP = LanePlanner()
self.last_cloudlog_t = 0
#self.steer_rate_cost = CP.steerRateCost
self.steer_rate_cost_prev = ntune_get('steerRateCost')
self.setup_mpc()
self.solution_invalid_cnt = 0
self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1'
self.auto_lane_change_enabled = Params().get('AutoLaneChangeEnabled') == b'1'
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.auto_lane_change_timer = 0.0
self.prev_torque_applied = False
def update_d_poly(self, v_ego, camera_offset):
# only offset left and right lane lines; offsetting p_poly does not make sense
camera_offset = ntune_get('cameraOffset') #neokii
self.l_poly[3] += CAMERA_OFFSET + camera_offset
self.r_poly[3] += CAMERA_OFFSET + camera_offset
# Find current lanewidth
self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob -
self.lane_width_certainty)
current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width -
self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly,
self.l_prob, self.r_prob, self.lane_width,
v_ego)
def update(self, sm, pm, CP, VM):
v_ego = sm['carState'].vEgo
angle_steers = sm['carState'].steeringAngle
active = sm['controlsState'].active
angle_offset = sm['liveParameters'].angleOffset
steerRateCost = ntune_get('steerRateCost')
if self.steer_rate_cost_prev != steerRateCost:
self.steer_rate_cost_prev = steerRateCost
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers -
angle_offset) / VM.sR
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
# Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1)
if ntune_isEnabled('useLiveSteerRatio'):
sr = max(sm['liveParameters'].steerRatio, 0.1)
else:
sr = max(ntune_get('steerRatio'), 0.1)
VM.update_params(x, sr)
curvature_factor = VM.curvature_factor(v_ego)
self.LP.parse_model(sm['model'])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (
not one_blinker) or (not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \
self.auto_lane_change_enabled and \
(AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME
blindspot_detected = (
(sm['carState'].leftBlindspot
and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot
and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and (not blindspot_detected
or self.prev_torque_applied):
self.lane_change_state = LaneChangeState.laneChangeStarting
elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0:
self.auto_lane_change_timer = 10.0
elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied:
self.prev_torque_applied = True
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(
self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(
self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [
LaneChangeState.off, LaneChangeState.preLaneChange
]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
if self.lane_change_state == LaneChangeState.off:
self.auto_lane_change_timer = 0.0
self.prev_torque_applied = False
elif self.auto_lane_change_timer < (
AUTO_LCA_START_TIME +
0.25): # stop afer 3 sec resume from 10 when torque applied
self.auto_lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft:
self.LP.l_prob *= self.lane_change_ll_prob
self.LP.r_prob *= self.lane_change_ll_prob
self.LP.update_d_poly(v_ego)
steerActuatorDelay = ntune_get('steerActuatorDelay')
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego,
angle_steers - angle_offset,
curvature_factor, VM.sR,
steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(self.LP.l_poly), list(self.LP.r_poly),
list(self.LP.d_poly), self.LP.l_prob,
self.LP.r_prob, curvature_factor, v_ego_mpc,
self.LP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
rate_desired = 0.0
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(
math.degrees(delta_desired * VM.sR) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE,
MPC_COST_LAT.HEADING, steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers -
angle_offset) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[
0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
plan_solution_valid = self.solution_invalid_cnt < 3
plan_send = messaging.new_message('pathPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=[
'carState', 'controlsState', 'liveParameters', 'model'
])
plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
plan_send.pathPlan.lProb = float(self.LP.l_prob)
plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
plan_send.pathPlan.rProb = float(self.LP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
plan_send.pathPlan.angleOffset = float(
sm['liveParameters'].angleOffset)
plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
plan_send.pathPlan.desire = desire
plan_send.pathPlan.laneChangeState = self.lane_change_state
plan_send.pathPlan.laneChangeDirection = self.lane_change_direction
plan_send.pathPlan.autoLaneChangeEnabled = self.auto_lane_change_enabled
plan_send.pathPlan.autoLaneChangeTimer = int(
AUTO_LCA_START_TIME) - int(self.auto_lane_change_timer)
plan_send.pathPlan.steerRatio = VM.sR
plan_send.pathPlan.steerRateCost = steerRateCost
plan_send.pathPlan.steerActuatorDelay = steerActuatorDelay
pm.send('pathPlan', plan_send)
if LOG_MPC:
dat = messaging.new_message('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
pm.send('liveMpc', dat)
def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = self.enabled
CC.actuators = actuators
# scc smoother
CC.cruiseOpMaxSpeed = self.cruiseOpMaxSpeed
CC.cruiseControl.override = True
CC.cruiseControl.cancel = self.CP.enableCruise and not self.enabled and CS.cruiseState.enabled
# Some override values for Honda
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
speed_override = max(0.0,
(self.LoC.v_pid + CS.cruiseState.speedOffset) *
brake_discount)
CC.cruiseControl.speedOverride = float(
speed_override if self.CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = self.CI.calc_accel_override(
CS.aEgo, self.sm['plan'].aTarget, CS.vEgo, self.sm['plan'].vTarget)
CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = self.enabled
CC.hudControl.lanesVisible = self.enabled
CC.hudControl.leadVisible = self.sm['plan'].hasLead
right_lane_visible = self.sm['pathPlan'].rProb > 0.5
left_lane_visible = self.sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (self.sm.frame - self.last_blinker_frame
) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
meta = self.sm['model'].meta
if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft -
1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight -
1]
cameraOffset = ntune_get("cameraOffset")
l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3]
< (1.08 - cameraOffset))
r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3]
> -(1.08 + cameraOffset))
CC.hudControl.leftLaneDepart = bool(
l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(
r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
self.events.add(EventName.ldw)
clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None
alerts = self.events.create_alerts(self.current_alert_types,
[self.CP, self.sm, self.is_metric])
self.AM.add_many(self.sm.frame, alerts, self.enabled)
self.AM.process_alerts(self.sm.frame, clear_event)
CC.hudControl.visualAlert = self.AM.visual_alert
if not self.read_only:
# send car controls over can
can_sends = self.CI.apply(CC, self)
self.pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = (self.sm['dMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
angleOffset = self.sm['pathPlan'].angleOffset
steer_angle_rad = (CS.steeringAngle - angleOffset) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.alertText1 = self.AM.alert_text_1
controlsState.alertText2 = self.AM.alert_text_2
controlsState.alertSize = self.AM.alert_size
controlsState.alertStatus = self.AM.alert_status
controlsState.alertBlinkingRate = self.AM.alert_rate
controlsState.alertType = self.AM.alert_type
controlsState.alertSound = self.AM.audible_alert
controlsState.driverMonitoringOn = self.sm[
'dMonitoringState'].faceDetected
controlsState.canMonoTimes = list(CS.canMonoTimes)
controlsState.planMonoTime = self.sm.logMonoTime['plan']
controlsState.pathPlanMonoTime = self.sm.logMonoTime['pathPlan']
controlsState.enabled = self.enabled
controlsState.active = self.active
controlsState.vEgo = CS.vEgo
controlsState.vEgoRaw = CS.vEgoRaw
controlsState.angleSteers = CS.steeringAngle - angleOffset
controlsState.curvature = self.VM.calc_curvature(
steer_angle_rad, CS.vEgo)
controlsState.steerOverride = CS.steeringPressed
controlsState.state = self.state
controlsState.engageable = not self.events.any(ET.NO_ENTRY)
controlsState.longControlState = self.LoC.long_control_state
controlsState.vPid = float(self.LoC.v_pid)
controlsState.vCruise = float(self.v_cruise_kph)
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.jerkFactor = float(self.sm['plan'].jerkFactor)
controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive
controlsState.vCurvature = self.sm['plan'].vCurvature
controlsState.decelForModel = self.sm[
'plan'].longitudinalPlanSource == LongitudinalPlanSource.model
controlsState.cumLagMs = -self.rk.remaining * 1000.
controlsState.startMonoTime = int(start_time * 1e9)
controlsState.mapValid = self.sm['plan'].mapValid
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.can_error_counter
if self.CP.lateralTuning.which() == 'pid':
controlsState.lateralControlState.pidState = lac_log
elif self.CP.lateralTuning.which() == 'lqr':
controlsState.lateralControlState.lqrState = lac_log
elif self.CP.lateralTuning.which() == 'indi':
controlsState.lateralControlState.indiState = lac_log
self.pm.send('controlsState', dat)
# carState
car_events = self.events.to_msg()
cs_send = messaging.new_message('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = car_events
self.pm.send('carState', cs_send)
# carEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL)
== 0) or (self.events.names != self.events_prev):
ce_send = messaging.new_message('carEvents', len(self.events))
ce_send.carEvents = car_events
self.pm.send('carEvents', ce_send)
self.events_prev = self.events.names.copy()
# carParams - logged every 50 seconds (> 1 per segment)
if (self.sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message('carParams')
cp_send.carParams = self.CP
self.pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
# Neokii's live tune
# Update VehicleModel
params = self.sm['liveParameters']
x = max(params.stiffnessFactor, 0.1)
#sr = max(params.steerRatio, 0.1)
if ntune_isEnabled('useLiveSteerRatio'):
sr = max(self.sm['liveParameters'].steerRatio, 0.1)
else:
if self.CP.carName in [CAR.VOLT]:
sr = interp(abs(self.angle_steers_des), [5., 35.],
[13.5, 17.7])
else:
sr = max(ntune_get('steerRatio'), 0.1)
self.VM.update_params(x, sr)
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
if CS.leftBlinker or CS.rightBlinker:
self.last_blinker_frame = self.sm.frame
# State specific actions
if not self.active:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
long_plan_age = DT_CTRL * (self.sm.frame -
self.sm.rcv_frame['longitudinalPlan'])
# no greater than dt mpc + dt, to prevent too high extraps
dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (
long_plan.aTarget - long_plan.aStart)
v_acc_sol = long_plan.vStart + dt * (a_acc_sol +
long_plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(
self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol,
self.CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(
self.active, CS, self.CP, self.VM, params, lat_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
else:
self.saturated_count = 0
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[
0] < -0.1
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[
0] > 0.1
# Bellow 2Lines' notations are for disable Alerts
# if left_deviation or right_deviation:
# self.events.add(EventName.steerSaturated)
return actuators, v_acc_sol, a_acc_sol, lac_log
def update(self, sm, CP):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
md = sm['modelV2']
self.LP.parse_model(sm['modelV2'])
if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.t_idxs = np.array(md.position.t)
self.plan_yaw = list(md.orientation.z)
if len(md.orientation.xStd) == TRAJECTORY_SIZE:
self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
# Lane change logic
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
if sm['carState'].leftBlinker:
self.lane_change_direction = LaneChangeDirection.left
elif sm['carState'].rightBlinker:
self.lane_change_direction = LaneChangeDirection.right
if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
else:
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) or \
self.auto_lane_change_enabled and \
(AUTO_LCA_START_TIME+0.25) > self.auto_lane_change_timer > AUTO_LCA_START_TIME
blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# off
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_ll_prob = 1.0
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied and (not blindspot_detected or self.prev_torque_applied):
self.lane_change_state = LaneChangeState.laneChangeStarting
elif torque_applied and blindspot_detected and self.auto_lane_change_timer != 10.0:
self.auto_lane_change_timer = 10.0
elif not torque_applied and self.auto_lane_change_timer == 10.0 and not self.prev_torque_applied:
self.prev_torque_applied = True
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
# 98% certainty
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
# fade in laneline over 1s
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if one_blinker and self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.preLaneChange
elif self.lane_change_ll_prob > 0.99:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
if self.lane_change_state == LaneChangeState.off:
self.auto_lane_change_timer = 0.0
self.prev_torque_applied = False
elif self.auto_lane_change_timer < (AUTO_LCA_START_TIME+0.25): # stop afer 3 sec resume from 10 when torque applied
self.auto_lane_change_timer += DT_MDL
self.prev_one_blinker = one_blinker
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Turn off lanes during lane change
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob
self.LP.rll_prob *= self.lane_change_ll_prob
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
self.libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, ntune_get('steerRateCost'))
else:
d_path_xyz = self.path_xyz
path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 5.0) * MPC_COST_LAT.PATH
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
self.libmpc.set_weights(path_cost, heading_cost, ntune_get('steerRateCost'))
y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
assert len(y_pts) == MPC_N + 1
assert len(heading_pts) == MPC_N + 1
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
float(v_ego),
CAR_ROTATION_RADIUS,
list(y_pts),
list(heading_pts))
# init state for next
self.cur_state.x = 0.0
self.cur_state.y = 0.0
self.cur_state.psi = 0.0
self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:MPC_N + 1], self.mpc_solution.curvature)
# TODO this needs more thought, use .2s extra for now to estimate other delays
delay = ntune_get('steerActuatorDelay') + .2
current_curvature = self.mpc_solution.curvature[0]
psi = interp(delay, self.t_idxs[:MPC_N + 1], self.mpc_solution.psi)
next_curvature_rate = self.mpc_solution.curvature_rate[0]
# MPC can plan to turn the wheel and turn back before t_delay. This means
# in high delay cases some corrections never even get commanded. So just use
# psi to calculate a simple linearization of desired curvature
curvature_diff_from_psi = psi / (max(v_ego, 1e-1) * delay) - current_curvature
next_curvature = current_curvature + 2 * curvature_diff_from_psi
self.desired_curvature = next_curvature
self.desired_curvature_rate = next_curvature_rate
max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES)
self.safe_desired_curvature_rate = clip(self.desired_curvature_rate,
-max_curvature_rate,
max_curvature_rate)
self.safe_desired_curvature = clip(self.desired_curvature,
self.safe_desired_curvature - max_curvature_rate/DT_MDL,
self.safe_desired_curvature + max_curvature_rate/DT_MDL)
# Check for infeasable MPC solution
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
t = sec_since_boot()
if mpc_nans:
self.libmpc.init()
self.cur_state.curvature = measured_curvature
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.solution_invalid_cnt += 1
else:
self.solution_invalid_cnt = 0
