def run_following_distance_simulation(v_lead, t_end=200.0):
dt = 0.2
t = 0.
x_lead = 200.0
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
pm = FakePubMaster()
mpc = LongitudinalMpc(1)
first = True
while t < t_end:
# Run cruise control
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, False)
]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0], dt)
# Setup CarState
CS = messaging.new_message('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.RadarState.LeadData.new_message()
lead.status = True
lead.dRel = x_lead - x_ego
lead.vLead = v_lead
lead.aLeadK = 0.0
# Run MPC
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS.carState, lead)
mpc.publish(pm)
mpc.update(CS.carState, lead)
mpc.publish(pm)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
v_ego, a_ego = v_cruise, a_cruise
else:
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
# Update state
x_lead += v_lead * dt
x_ego += v_ego * dt
t += dt
first = False
return x_lead - x_ego
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.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
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
def choose_solution(self, v_cruise_setpoint, 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
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
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
def update(self, sm, CP):
"""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.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# 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']
pm.send('longitudinalPlan', plan_send)
class Planner():
def __init__(self, CP):
self.CP = CP
self.mpc1 = LongitudinalMpc(1)
self.mpc2 = LongitudinalMpc(2)
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.source = Source.cruiseCoast
self.cruise_source = Source.cruiseCoast
self.fcw_checker = FCWChecker()
self.path_x = np.arange(192)
self.fcw = False
self.params = Params()
self.kegman = kegman_conf()
self.mpc_frame = 0
self.first_loop = True
self.coast_enabled = True
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {self.cruise_source: self.v_cruise}
if self.mpc1.prev_lead_status:
solutions[Source.mpc1] = self.mpc1.v_mpc
if self.mpc2.prev_lead_status:
solutions[Source.mpc2] = self.mpc2.v_mpc
slowest = min(solutions, key=solutions.get)
self.source = slowest
# Choose lowest of MPC and cruise
if slowest == Source.mpc1:
self.v_acc = self.mpc1.v_mpc
self.a_acc = self.mpc1.a_mpc
elif slowest == Source.mpc2:
self.v_acc = self.mpc2.v_mpc
self.a_acc = self.mpc2.a_mpc
elif slowest == self.cruise_source:
self.v_acc = self.v_cruise
self.a_acc = self.a_cruise
self.v_acc_future = min([
self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint
])
def choose_cruise(self, v_ego, a_ego, v_cruise_setpoint,
accel_limits_turns, jerk_limits, gasbrake):
# WARNING: Logic is carefully verified. On change, review test_longitudinal.py output!
# Standard cruise
if not self.coast_enabled:
self.cruise_source = Source.cruiseGas
return 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)
# If coasting, reset starting state for gas and brake plans
if self.source == Source.cruiseCoast:
self.v_acc_start = v_ego
self.a_acc_start = a_ego
elif self.source in [Source.mpc1, Source.mpc2]:
self.cruise_source = Source.cruiseGas if gasbrake >= 0 else Source.cruiseBrake
# Coast to (current state)
v_coast, a_coast = v_ego, a_ego
# Gas to (v_cruise_setpoint)
v_gas, a_gas = 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)
# Brake to (v_cruise_setpoint + COAST_SPEED)
v_brake, a_brake = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint + COAST_SPEED,
accel_limits_turns[1],
accel_limits_turns[0],
jerk_limits[1], jerk_limits[0],
LON_MPC_STEP)
cruise = {
Source.cruiseCoast: (v_coast, a_coast),
Source.cruiseGas: (v_gas, a_gas),
Source.cruiseBrake: (v_brake, a_brake),
}
# Entry conditions
if gasbrake == 0:
if a_brake <= a_coast:
self.cruise_source = Source.cruiseBrake
elif a_gas >= a_coast:
self.cruise_source = Source.cruiseGas
elif (a_brake >= a_coast >= a_gas):
self.cruise_source = Source.cruiseCoast
return cruise[self.cruise_source]
def update(self, sm, CP, VM, PP):
"""Gets called when new radarState is available"""
cur_time = sec_since_boot()
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
gasbrake = sm['carControl'].actuators.gas - sm[
'carControl'].actuators.brake
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 in [
LongCtrlState.pid, 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 self.mpc_frame % 1000 == 0:
self.kegman = kegman_conf()
self.mpc_frame = 0
self.mpc_frame += 1
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, self.kegman.conf['accelerationMode'])
]
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 = accel_limits # 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 = self.choose_cruise(
v_ego, a_ego, v_cruise_setpoint, accel_limits_turns,
jerk_limits, gasbrake)
# 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(a_ego, 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.cruise_source = Source.cruiseCoast
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.mpc1.update(sm['carState'], lead_1)
self.mpc2.update(sm['carState'], lead_2)
self.choose_solution(v_cruise_setpoint, enabled)
# 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.source
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.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)
def run_following_distance_simulation(TR_override, v_lead, t_end=200.0):
dt = 0.2
t = 0.
lead_pos = LeadPos(v_lead)
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
pm = FakePubMaster()
mpc = LongitudinalMpc(1, TR_override)
datapoints = [{'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}]
first = True
while t < t_end:
# Run cruise control
accel_limits = [
float(x) for x in calc_cruise_accel_limits(v_ego, False)
]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0], dt)
# Setup CarState
CS = messaging.new_message('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.RadarState.LeadData.new_message()
lead.status = True
lead.dRel = lead_pos.x - x_ego
lead.vLead = lead_pos.v
lead.aLeadK = 0.0
# Run MPC
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS.carState, lead)
mpc.publish(pm)
mpc.update(CS.carState, lead)
mpc.publish(pm)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
v_ego, a_ego = v_cruise, a_cruise
else:
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
# Update state
lead_pos.update(t, dt)
x_ego += v_ego * dt
t += dt
first = False
datapoints.append({'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x})
filename = f'test_out/{v_lead}_{TR_override}.json'
with open(filename, 'w') as datafile:
json.dump(datapoints, datafile)
return lead_pos.x - x_ego
