class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
self.target_speed_map = 0.0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
self.target_speed_map_counter1 = 0
self.target_speed_map_counter2 = 0
self.target_speed_map_counter3 = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
self.target_speed_map_sign = False
self.tartget_speed_offset = int(self.params.get("OpkrSpeedLimitOffset", encoding="utf8"))
self.vego = 0
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(self.mpc_model.v_mpc_future) # only used when using model
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
self.v_acc_future = min(possible_futures)
def update(self, sm, CP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
self.vego = v_ego
long_control_state = sm['controlsState'].longControlState
if self.params.get_bool("OpenpilotLongitudinalControl"):
v_cruise_kph = sm['carState'].vSetDis
else:
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
if self.params.get_bool("OpkrMapEnable"):
self.target_speed_map_counter += 1
if self.target_speed_map_counter >= (50+self.target_speed_map_counter1) and self.target_speed_map_counter_check == False:
self.target_speed_map_counter_check = True
os.system("logcat -d -s opkrspdlimit,opkrspd2limit | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCamera &")
os.system("logcat -d -s opkrspddist | grep opkrspd | tail -n 1 | awk \'{print $7}\' > /data/params/d/LimitSetSpeedCameraDist &")
self.target_speed_map_counter3 += 1
if self.target_speed_map_counter3 > 2:
self.target_speed_map_counter3 = 0
os.system("logcat -c &")
elif self.target_speed_map_counter >= (75+self.target_speed_map_counter1):
self.target_speed_map_counter1 = 0
self.target_speed_map_counter = 0
self.target_speed_map_counter_check = False
mapspeed = self.params.get("LimitSetSpeedCamera", encoding="utf8")
mapspeeddist = self.params.get("LimitSetSpeedCameraDist", encoding="utf8")
if mapspeed is not None and mapspeeddist is not None:
mapspeed = int(float(mapspeed.rstrip('\n')))
mapspeeddist = int(float(mapspeeddist.rstrip('\n')))
if mapspeed > 29:
self.target_speed_map = mapspeed
self.target_speed_map_dist = mapspeeddist
if self.target_speed_map_dist > 1001:
self.target_speed_map_block = True
self.target_speed_map_counter1 = 80
os.system("logcat -c &")
else:
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_block = False
elif mapspeed is None and mapspeeddist is None and self.target_speed_map_counter2 < 2:
self.target_speed_map_counter2 += 1
self.target_speed_map_counter = 51
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = True
self.target_speed_map_block = False
self.target_speed_map_sign = False
else:
self.target_speed_map_counter = 49
self.target_speed_map_counter2 = 0
self.target_speed_map = 0
self.target_speed_map_dist = 0
self.target_speed_map_counter_check = False
self.target_speed_map_block = False
self.target_speed_map_sign = False
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].brakePressed and not sm['carState'].gasPressed:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel and False: # awareness decel is disabled for now
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
distances, speeds, accelerations = self.model_mpc_helper.convert_data(sm)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
distances,
speeds,
accelerations)
self.choose_solution(v_cruise_setpoint, enabled, self.params.get_bool("ModelLongEnabled"))
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time,
sm['controlsState'].active,
v_ego, sm['carState'].aEgo,
lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
lead_1.yRel, lead_1.vLat,
lead_1.fcw, blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def publish(self, sm, pm):
self.mpc1.publish(pm)
self.mpc2.publish(pm)
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.rcv_time['radarState']
# Send radarstate(dRel, vRel, yRel)
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
longitudinalPlan.dRel1 = float(lead_1.dRel)
longitudinalPlan.yRel1 = float(lead_1.yRel)
longitudinalPlan.vRel1 = float(lead_1.vRel)
longitudinalPlan.dRel2 = float(lead_2.dRel)
longitudinalPlan.yRel2 = float(lead_2.yRel)
longitudinalPlan.vRel2 = float(lead_2.vRel)
longitudinalPlan.status2 = bool(lead_2.status)
cam_distance_calc = 0
cam_distance_calc = interp(self.vego*CV.MS_TO_KPH, [30,60,100,160], [3.75,5.5,6,7])
consider_speed = interp((self.vego*CV.MS_TO_KPH - self.target_speed_map), [10, 30], [1, 1.3])
if self.target_speed_map > 29 and self.target_speed_map_dist < cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_dist >= cam_distance_calc*consider_speed*self.vego*CV.MS_TO_KPH and self.target_speed_map_block:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
self.target_speed_map_sign = True
elif self.target_speed_map > 29 and self.target_speed_map_sign:
longitudinalPlan.targetSpeedCamera = float(self.target_speed_map)
longitudinalPlan.targetSpeedCameraDist = float(self.target_speed_map_dist)
else:
longitudinalPlan.targetSpeedCamera = 0
longitudinalPlan.targetSpeedCameraDist = 0
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.model_mpc_helper = ModelMpcHelper()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc_next = 0.0
self.a_acc_next = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.solution = Solution(0., 0.)
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.first_loop = True
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled,
speed):
possible_futures = [
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
]
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(
self.mpc_model.v_mpc_future) # only used when using model
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
if self.op_params.get('accel_lag_compensation'):
#accel_delay = interp(speed * CV.MS_TO_MPH, [10, 80], [0.2, 0.4]) #Original
#accel_delay = interp(speed * CV.MS_TO_KPH, [10, 150], [1.6, 1.7]) # Last value, worked OK
accel_delay = interp(speed * CV.MS_TO_KPH, [10, 80], [.6, .8])
else:
accel_delay = 0.
# Some notes: a_acc_start should always be current timestep (or delayed)
# a_acc should be a_acc_start but +0.2 seconds so controlsd interps properly (a_acc_start to a_acc_start+0.05sec)
# If planner lags for up to ~0.15 seconds, controlsd can interp from 0.05 to 0.21 seconds
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
cur, fut = interp([accel_delay, accel_delay + 0.2],
MPC_TIMESTEPS,
self.mpc1.mpc_solution[0].a_ego)
self.solution = Solution(a_acc_start=cur, a_acc=fut)
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
cur, fut = interp([accel_delay, accel_delay + 0.2],
MPC_TIMESTEPS,
self.mpc2.mpc_solution[0].a_ego)
self.solution = Solution(a_acc_start=cur, a_acc=fut)
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.solution = Solution(
a_acc_start=self.a_cruise,
a_acc=self.a_cruise) # cruise doesn't matter
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
cur, fut = interp([accel_delay, accel_delay + 0.2],
MPC_TIMESTEPS,
self.mpc_model.mpc_solution[0].a_ego)
self.solution = Solution(a_acc_start=cur, a_acc=fut)
self.v_acc_future = min(possible_futures)
def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
self.v_acc_start = self.v_acc_next
self.a_acc_start = self.a_acc_next
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
distances, speeds, accelerations = self.model_mpc_helper.convert_data(
sm)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
distances, speeds, accelerations)
self.choose_solution(v_cruise_setpoint, enabled,
sm['modelLongButton'].enabled, v_ego)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_next = v_acc_sol
self.a_acc_next = a_acc_sol
self.first_loop = False
def publish(self, sm, pm):
self.mpc1.publish(pm)
self.mpc2.publish(pm)
plan_send = messaging.new_message('longitudinalPlan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.mdMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.radarStateMonoTime = sm.logMonoTime['radarState']
longitudinalPlan.vCruise = float(self.v_cruise)
longitudinalPlan.aCruise = float(self.a_cruise)
longitudinalPlan.vStart = float(self.v_acc_start)
longitudinalPlan.aStart = float(self.solution.a_acc_start)
longitudinalPlan.vTarget = float(self.v_acc)
longitudinalPlan.aTarget = float(self.solution.a_acc)
longitudinalPlan.vTargetFuture = float(self.v_acc_future)
longitudinalPlan.hasLead = self.mpc1.prev_lead_status
longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
longitudinalPlan.fcw = self.fcw
longitudinalPlan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future,
self.mpc_model.v_mpc_future, v_cruise_setpoint
])
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm[
'carState'].gasPressed: # gasPress is to avoid hard decel after user accelerates with gas while engaged
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
sm['model'].longitudinal.distances,
sm['model'].longitudinal.speeds,
sm['model'].longitudinal.accelerations)
self.choose_solution(v_cruise_setpoint, enabled,
sm['modelLongButton'].enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
possible_futures = [
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
]
if enabled:
solutions = {
'cruise': self.v_cruise,
'curveSlowdown': self.v_model
}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(
self.mpc_model.v_mpc_future) # only used when using model
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
elif slowest == 'curveSlowdown':
self.v_acc = self.v_model
self.a_acc = self.a_model
self.v_acc_future = min(possible_futures)
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if len(sm['model'].path.poly): # old slowdown for curves code
path = list(sm['model'].path.poly)
# Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
# y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
# k = y'' / (1 + y'^2)^1.5
# TODO: compute max speed without using a list of points and without numpy
y_p = 3 * path[0] * self.path_x**2 + 2 * path[
1] * self.path_x + path[2]
y_pp = 6 * path[0] * self.path_x + 2 * path[1]
curv = y_pp / (1. + y_p**2)**1.5
a_y_max = 3.1 - v_ego * 0.032
v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
model_speed = np.min(v_curvature)
model_speed = max(20.0 * CV.MPH_TO_MS,
model_speed) # Don't slow down below 20mph
else:
model_speed = 255. # (MAX_SPEED)
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, following)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
self.v_model, self.a_model = speed_smoother(
self.v_acc_start, self.a_acc_start, model_speed,
2 * accel_limits[1], accel_limits[0], 2 * jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
sm['model'].longitudinal.distances,
sm['model'].longitudinal.speeds,
sm['model'].longitudinal.accelerations)
self.choose_solution(v_cruise_setpoint, enabled,
sm['modelLongButton'].enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.osm = True
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
self.offset = 0
self.last_time = 0
def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2,
steeringAngle):
center_x = -2.5 # Wheel base 2.5m
lead1_check = True
mpc_offset = opParams().get('mpc_offset', default=5.0)
lead2_check = True
if steeringAngle > 100: # only at high angles
center_y = -1 + 2.5 / math.tan(
steeringAngle / 1800. * math.pi
) # Car Width 2m. Left side considered in left hand turn
lead1_check = math.sqrt(
(lead_1.dRel - center_x)**2 +
(lead_1.yRel - center_y)**2) < abs(
2.5 / math.sin(steeringAngle / 1800. * math.pi)
) + 1. # extra meter clearance to car
lead2_check = math.sqrt(
(lead_2.dRel - center_x)**2 +
(lead_2.yRel - center_y)**2) < abs(
2.5 / math.sin(steeringAngle / 1800. * math.pi)) + 1.
elif steeringAngle < -100: # only at high angles
center_y = +1 + 2.5 / math.tan(
steeringAngle / 1800. * math.pi
) # Car Width 2m. Right side considered in right hand turn
lead1_check = math.sqrt(
(lead_1.dRel - center_x)**2 +
(lead_1.yRel - center_y)**2) < abs(
2.5 / math.sin(steeringAngle / 1800. * math.pi)) + 1.
lead2_check = math.sqrt(
(lead_2.dRel - center_x)**2 +
(lead_2.yRel - center_y)**2) < abs(
2.5 / math.sin(steeringAngle / 1800. * math.pi)) + 1.
if enabled:
solutions = {}
if self.mpc1.prev_lead_status and lead1_check:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status and lead2_check:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid:
if self.mpc_model.v_mpc > 13.0:
solutions['model'] = NO_CURVATURE_SPEED
else:
solutions['model'] = self.mpc_model.v_mpc + mpc_offset
solutions['cruise'] = self.v_cruise
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc + mpc_offset
self.a_acc = self.mpc_model.a_mpc
print("Model Speed kph")
print(self.mpc_model.v_mpc * 3.6)
self.v_acc_future = v_cruise_setpoint
if lead1_check:
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.v_acc_future,
self.mpc_model.v_mpc_future + mpc_offset
])
if lead2_check:
self.v_acc_future = min([
self.mpc2.v_mpc_future, self.v_acc_future,
self.mpc_model.v_mpc_future + mpc_offset
])
def update(self, sm, pm, CP, VM, PP, arne_sm):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
# we read offset value every 5 seconds
fixed_offset = 0.0
if not travis:
fixed_offset = op_params.get('speed_offset', 0.0)
if self.last_time > 5:
try:
self.offset = int(
self.params.get("SpeedLimitOffset", encoding='utf8'))
except (TypeError, ValueError):
self.params.delete("SpeedLimitOffset")
self.offset = 0
self.osm = self.params.get("LimitSetSpeed",
encoding='utf8') == "1"
self.last_time = 0
self.last_time = self.last_time + 1
gas_button_status = arne_sm['arne182Status'].gasbuttonstatus
v_ego = sm['carState'].vEgo
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
if blinkers:
steering_angle = sm['carState'].steeringAngle * 0.8
if v_ego < 11:
angle_later = 0.
else:
angle_later = arne_sm['latControl'].anglelater * 0.8
else:
steering_angle = sm['carState'].steeringAngle
if v_ego < 11:
angle_later = 0.
else:
angle_later = arne_sm['latControl'].anglelater
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = (lead_1.status and lead_1.dRel < 40.0
and lead_1.vRel < 0.0) or (lead_2.status
and lead_2.dRel < 40.0
and lead_2.vRel < 0.0)
if gas_button_status == 1:
speed_ahead_distance = 150
elif gas_button_status == 2:
speed_ahead_distance = 350
else:
speed_ahead_distance = default_brake_distance
v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
v_speedlimit_ahead = NO_CURVATURE_SPEED
now = datetime.now()
try:
if sm['liveMapData'].speedLimitValid and osm and self.osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
speed_limit = sm['liveMapData'].speedLimit
if speed_limit is not None:
v_speedlimit = speed_limit
# offset is in percentage,.
if v_ego > offset_limit:
v_speedlimit = v_speedlimit * (1. +
self.offset / 100.0)
if v_speedlimit > fixed_offset:
v_speedlimit = v_speedlimit + fixed_offset
else:
speed_limit = None
if sm['liveMapData'].speedLimitAheadValid and osm and self.osm and sm[
'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
distanceatlowlimit = 50
if sm['liveMapData'].speedLimitAhead < 21 / 3.6:
distanceatlowlimit = speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2
if distanceatlowlimit < 50:
distanceatlowlimit = 0
distanceatlowlimit = min(distanceatlowlimit, 100)
speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5
speed_ahead_distance = min(speed_ahead_distance, 300)
speed_ahead_distance = max(speed_ahead_distance, 50)
if speed_limit is not None and sm[
'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[
'liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead
) * sm[
'liveMapData'].speedLimitAheadDistance / speed_ahead_distance:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead) * (
sm['liveMapData'].speedLimitAheadDistance -
distanceatlowlimit) / (speed_ahead_distance -
distanceatlowlimit)
else:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead
if speed_limit_ahead is not None:
v_speedlimit_ahead = speed_limit_ahead
if v_ego > offset_limit:
v_speedlimit_ahead = v_speedlimit_ahead * (
1. + self.offset / 100.0)
if v_speedlimit_ahead > fixed_offset:
v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset
if sm['liveMapData'].curvatureValid and sm[
'liveMapData'].distToTurn < speed_ahead_distance and osm and self.osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
curvature = abs(sm['liveMapData'].curvature)
radius = 1 / max(1e-4, curvature) * curvature_factor
if gas_button_status == 1:
radius = radius * 2.0
elif gas_button_status == 2:
radius = radius * 1.0
else:
radius = radius * 1.5
if radius > 500:
c = 0.9 # 0.9 at 1000m = 108 kph
elif radius > 250:
c = 3.5 - 13 / 2500 * radius # 2.2 at 250m 84 kph
else:
c = 3.0 - 2 / 625 * radius # 3.0 at 15m 24 kph
v_curvature_map = math.sqrt(c * radius)
v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map)
except KeyError:
pass
decel_for_turn = bool(v_curvature_map < min(
[v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm[
'carState'].brakePressed and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(
v_ego, following and
(self.longitudinalPlanSource == 'mpc1' or
self.longitudinalPlanSource == 'mpc2'), gas_button_status)
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, steering_angle,
accel_limits, self.CP,
angle_later)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
if decel_for_turn and sm[
'liveMapData'].distToTurn < speed_ahead_distance and not following:
time_to_turn = max(
1.0, sm['liveMapData'].distToTurn / max(
(v_ego + v_curvature_map) / 2, 1.))
required_decel = min(0,
(v_curvature_map - v_ego) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm[
'liveMapData'].speedLimitAheadDistance > 1.0 and not following:
required_decel = min(
0,
(v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) /
(sm['liveMapData'].speedLimitAheadDistance * 2))
required_decel = max(required_decel, -3.0)
decel_for_turn = True
accel_limits[0] = required_decel
accel_limits[1] = required_decel
self.a_acc_start = required_decel
v_speedlimit_ahead = v_ego
v_cruise_setpoint = min([
v_cruise_setpoint, v_curvature_map, v_speedlimit,
v_speedlimit_ahead
])
#if (self.mpc1.prev_lead_status and self.mpc1.v_mpc < v_ego*0.99) or (self.mpc2.prev_lead_status and self.mpc2.v_mpc < v_ego*0.99):
# v_cruise_setpoint = v_ego
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint)
self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].vEgo,
sm['model'].longitudinal.distances,
sm['model'].longitudinal.speeds,
sm['model'].longitudinal.accelerations)
self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2,
sm['carState'].steeringAngle)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = float(v_curvature_map)
plan_send.plan.decelForTurn = bool(decel_for_turn)
plan_send.plan.mapValid = True
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False
class Planner():
def __init__(self, CP):
self.CP = CP
self.op_params = opParams()
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
self.mpc_model = LongitudinalMpcModel()
self.v_acc_start = 0.0
self.a_acc_start = 0.0
self.v_acc = 0.0
self.v_acc_future = 0.0
self.a_acc = 0.0
self.v_cruise = 0.0
self.a_cruise = 0.0
self.v_model = 0.0
self.a_model = 0.0
self.osm = False
self.osm_curv = False
self.osm_camera = False
self.longitudinalPlanSource = 'cruise'
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
self.offset = 0
self.last_time = 0
def choose_solution(self, v_cruise_setpoint, enabled, model_enabled):
possible_futures = [
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
]
if enabled:
solutions = {'cruise': self.v_cruise}
if self.mpc1.prev_lead_status:
solutions['mpc1'] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions['mpc2'] = self.mpc2.v_mpc
if self.mpc_model.valid and model_enabled:
solutions['model'] = self.mpc_model.v_mpc
possible_futures.append(
self.mpc_model.v_mpc_future) # only used when using model
slowest = min(solutions, key=solutions.get)
self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise
if slowest == 'mpc1':
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == 'mpc2':
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == 'cruise':
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
elif slowest == 'model':
self.v_acc = self.mpc_model.v_mpc
self.a_acc = self.mpc_model.a_mpc
self.v_acc_future = min(possible_futures)
def parse_modelV2_data(self, sm):
modelV2 = sm['modelV2']
distances, speeds, accelerations = [], [], []
if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
return distances, speeds, accelerations
model_t = modelV2.position.t
mpc_times = 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_times
] # matches 0 to 9 interval to idx from t
for t in model_t_idx: # everything is derived from x position since velocity is outputting weird values
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])
accelerations.insert(
0, accelerations[1] - (accelerations[2] - accelerations[1])
) # extrapolate back first accel from second and third, less weight
return distances, speeds, accelerations
def update(self, sm, pm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
# we read offset value every 5 seconds
fixed_offset = 0.0
fixed_offset = op_params.get('speed_offset')
if self.last_time > 5:
try:
self.offset = int(
self.params.get("SpeedLimitOffset", encoding='utf8'))
except (TypeError, ValueError):
self.params.delete("SpeedLimitOffset")
self.offset = 0
self.osm = self.params.get("LimitSetSpeed", encoding='utf8') == "1"
self.osm_curv = self.params.get("LimitSetSpeedCurv",
encoding='utf8') == "1"
self.osm_camera = self.params.get("LimitSetSpeedCamera",
encoding='utf8') == "1"
self.last_time = 0
self.last_time = self.last_time + 1
gas_button_status = int(self.params.get('OpkrAccMode'))
if eco_mode and gas_button_status == 0:
gas_button_status = 2
v_ego = sm['carState'].vEgo
long_control_state = sm['controlsState'].longControlState
v_cruise_kph = sm['controlsState'].vCruise
force_slow_decel = sm['controlsState'].forceDecel
v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
lead_1 = sm['radarState'].leadOne
lead_2 = sm['radarState'].leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = lead_1.status and lead_1.dRel < 50.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
if gas_button_status == 1:
speed_ahead_distance = 150
elif gas_button_status == 2:
if eco_mode:
speed_ahead_distance = default_brake_distance
else:
speed_ahead_distance = 350
else:
speed_ahead_distance = default_brake_distance
v_speedlimit = NO_CURVATURE_SPEED
v_curvature_map = NO_CURVATURE_SPEED
v_speedlimit_ahead = NO_CURVATURE_SPEED
now = datetime.now()
try:
if sm['liveMapData'].speedLimitValid and osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
speed_limit = sm['liveMapData'].speedLimit
if speed_limit is not None:
v_speedlimit = speed_limit
# offset is in percentage,.
if v_ego > offset_limit:
v_speedlimit = v_speedlimit * (1. +
self.offset / 100.0)
if v_speedlimit > fixed_offset:
v_speedlimit = v_speedlimit + fixed_offset
else:
speed_limit = None
if sm['liveMapData'].speedLimitAheadValid and osm and sm[
'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000 and (
smart_speed or smart_speed_max_vego > v_ego):
distanceatlowlimit = 50
if sm['liveMapData'].speedLimitAhead < 21 / 3.6:
distanceatlowlimit = speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 2
if distanceatlowlimit < 50:
distanceatlowlimit = 0
distanceatlowlimit = min(distanceatlowlimit, 100)
speed_ahead_distance = (
v_ego - sm['liveMapData'].speedLimitAhead) * 3.6 * 5
speed_ahead_distance = min(speed_ahead_distance, 300)
speed_ahead_distance = max(speed_ahead_distance, 50)
if speed_limit is not None and sm[
'liveMapData'].speedLimitAheadDistance > distanceatlowlimit and v_ego + 3 < sm[
'liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead
) * sm[
'liveMapData'].speedLimitAheadDistance / speed_ahead_distance:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead + (
speed_limit - sm['liveMapData'].speedLimitAhead) * (
sm['liveMapData'].speedLimitAheadDistance -
distanceatlowlimit) / (speed_ahead_distance -
distanceatlowlimit)
else:
speed_limit_ahead = sm['liveMapData'].speedLimitAhead
if speed_limit_ahead is not None:
v_speedlimit_ahead = speed_limit_ahead
if v_ego > offset_limit:
v_speedlimit_ahead = v_speedlimit_ahead * (
1. + self.offset / 100.0)
if v_speedlimit_ahead > fixed_offset:
v_speedlimit_ahead = v_speedlimit_ahead + fixed_offset
if sm['liveMapData'].curvatureValid and sm[
'liveMapData'].distToTurn < speed_ahead_distance and osm and (
sm['liveMapData'].lastGps.timestamp -
time.mktime(now.timetuple()) * 1000) < 10000:
curvature = abs(sm['liveMapData'].curvature)
radius = 1 / max(1e-4, curvature) * curvature_factor
if gas_button_status == 1:
radius = radius * 2.0
elif gas_button_status == 2:
radius = radius * 1.0
else:
radius = radius * 1.5
if radius > 500:
c = 0.9 # 0.9 at 1000m = 108 kph
elif radius > 250:
c = 3.5 - 13 / 2500 * radius # 2.2 at 250m 84 kph
else:
c = 3.0 - 2 / 625 * radius # 3.0 at 15m 24 kph
v_curvature_map = math.sqrt(c * radius)
v_curvature_map = min(NO_CURVATURE_SPEED, v_curvature_map)
except KeyError:
pass
decel_for_turn = bool(v_curvature_map < min(
[v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
# Calculate speed for normal cruise control
if enabled and not self.first_loop and not sm[
'carState'].brakePressed and not sm['carState'].gasPressed:
accel_limits = [
float(x) for x in calc_cruise_accel_limits(
v_ego, following, int(self.params.get('OpkrAccMode')))
]
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(
v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits_turns[1] = min(accel_limits_turns[1],
AWARENESS_DECEL)
accel_limits_turns[0] = min(accel_limits_turns[0],
accel_limits_turns[1])
if decel_for_turn and sm[
'liveMapData'].distToTurn < speed_ahead_distance and not following:
time_to_turn = max(
1.0, sm['liveMapData'].distToTurn / max(
(v_ego + v_curvature_map) / 2, 1.))
required_decel = min(0,
(v_curvature_map - v_ego) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
if v_speedlimit_ahead < v_speedlimit and v_ego > v_speedlimit_ahead and sm[
'liveMapData'].speedLimitAheadDistance > 1.0 and not following:
required_decel = min(
0,
(v_speedlimit_ahead * v_speedlimit_ahead - v_ego * v_ego) /
(sm['liveMapData'].speedLimitAheadDistance * 2))
required_decel = max(required_decel, -3.0)
decel_for_turn = True
accel_limits[0] = required_decel
accel_limits[1] = required_decel
self.a_acc_start = required_decel
v_speedlimit_ahead = v_ego
if (
v_speedlimit_ahead * CV.MS_TO_KPH
) >= 30 and self.osm_camera and not self.osm_curv and not self.osm:
v_cruise_setpoint = min(
[v_cruise_setpoint, v_speedlimit_ahead])
elif self.osm_curv and not self.osm_camera and not self.osm:
v_cruise_setpoint = min([v_cruise_setpoint, v_curvature_map])
elif self.osm and not self.osm_curv and not self.osm_camera:
v_cruise_setpoint = min([v_cruise_setpoint, v_speedlimit])
elif self.osm_camera and self.osm_curv and not self.osm:
v_cruise_setpoint = min(
[v_cruise_setpoint, v_speedlimit_ahead, v_curvature_map])
elif self.osm_camera and not self.osm_curv and self.osm:
v_cruise_setpoint = min(
[v_cruise_setpoint, v_speedlimit_ahead, v_speedlimit])
elif not self.osm_camera and self.osm_curv and self.osm:
v_cruise_setpoint = min(
[v_cruise_setpoint, v_curvature_map, v_speedlimit])
elif self.osm_camera and self.osm_curv and self.osm:
v_cruise_setpoint = min([
v_cruise_setpoint, v_speedlimit_ahead, v_curvature_map,
v_speedlimit
])
else:
v_cruise_setpoint = min([v_cruise_setpoint])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits_turns[1], accel_limits_turns[0], jerk_limits[1],
jerk_limits[0], LON_MPC_STEP)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(sm['carState'].aEgo, 0.0)
reset_speed = self.CP.minSpeedCan if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(pm, sm['carState'], lead_1)
self.mpc2.update(pm, sm['carState'], lead_2)
distances, speeds, accelerations = self.parse_modelV2_data(sm)
self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo,
distances, speeds, accelerations)
self.choose_solution(v_cruise_setpoint, enabled,
sm['modelLongButton'].enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, sm['controlsState'].active,
v_ego, sm['carState'].aEgo, lead_1.dRel, lead_1.vLead,
lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw,
blinkers) and not sm['carState'].brakePressed
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
radar_dead = not sm.alive['radarState']
radar_errors = list(sm['radarState'].radarErrors)
radar_fault = car.RadarData.Error.fault in radar_errors
radar_can_error = car.RadarData.Error.canError in radar_errors
# **** send the plan ****
plan_send = messaging.new_message('plan')
plan_send.valid = sm.all_alive_and_valid(
service_list=['carState', 'controlsState', 'radarState'])
plan_send.plan.mdMonoTime = sm.logMonoTime['model']
plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']
# longitudal plan
plan_send.plan.vCruise = float(self.v_cruise)
plan_send.plan.aCruise = float(self.a_cruise)
plan_send.plan.vStart = float(self.v_acc_start)
plan_send.plan.aStart = float(self.a_acc_start)
plan_send.plan.vTarget = float(self.v_acc)
plan_send.plan.aTarget = float(self.a_acc)
plan_send.plan.vTargetFuture = float(self.v_acc_future)
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
a_error = round(self.a_acc - sm['carState'].aEgo, 2)
if abs(a_error) > 0.2:
print(f"-------- a_error: {a_error} --------")
plan_send.plan.vCurvature = float(v_curvature_map)
plan_send.plan.decelForTurn = bool(decel_for_turn)
plan_send.plan.mapValid = True
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCanError = bool(radar_can_error)
plan_send.plan.processingDelay = (plan_send.logMonoTime /
1e9) - sm.rcv_time['radarState']
# Send out fcw
plan_send.plan.fcw = fcw
# Send radarstate(dRel, vRel, yRel)
plan_send.plan.dRel1 = lead_1.dRel
plan_send.plan.yRel1 = lead_1.yRel
plan_send.plan.vRel1 = lead_1.vRel
plan_send.plan.dRel2 = lead_2.dRel
plan_send.plan.yRel2 = lead_2.yRel
plan_send.plan.vRel2 = lead_2.vRel
plan_send.plan.status2 = lead_2.status
plan_send.plan.targetSpeed = v_cruise_setpoint * CV.MS_TO_KPH
plan_send.plan.targetSpeedCamera = v_speedlimit_ahead * CV.MS_TO_KPH
pm.send('plan', plan_send)
# Interpolate 0.05 seconds and save as starting point for next iteration
a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (
self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (
a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False