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