class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.lat_Control = messaging.sub_sock(context, service_list['latControl'].port, conflate=True, poller=self.poller) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( context, service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) 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.fcw_enabled = fcw_enabled self.lastlat_Control = None self.params = Params() 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, CS, CP, VM, PP, live20, live100, md, live_map_data): """Gets called when new live20 is available""" cur_time = live20.logMonoTime / 1e9 v_ego = CS.carState.vEgo gasbuttonstatus = CS.carState.gasbuttonstatus long_control_state = live100.live100.longControlState v_cruise_kph = live100.live100.vCruise force_slow_decel = live100.live100.forceDecel v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS for socket, event in self.poller.poll(0): if socket is self.lat_Control: self.lastlat_Control = messaging.recv_one(socket).latControl self.lead_1 = live20.live20.leadOne self.lead_2 = live20.live20.leadTwo lead_1 = live20.live20.leadOne lead_2 = live20.live20.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 v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED map_valid = live_map_data.liveMapData.mapValid # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt( a_y_max / max(1e-4, curvature)) / 1.3 * _brake_factor v_curvature = min(NO_CURVATURE_SPEED, v_curvature) decel_for_turn = bool( v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = map( float, calc_cruise_accel_limits(v_ego, following, gasbuttonstatus)) if gasbuttonstatus == 0: accellimitmaxdynamic = -0.0018 * v_ego + 0.2 jerk_limits = [ min(-0.1, accel_limits[0] * 0.5), max(accellimitmaxdynamic, accel_limits[1]) ] # dynamic elif gasbuttonstatus == 1: accellimitmaxsport = -0.002 * v_ego + 0.4 jerk_limits = [ min(-0.25, accel_limits[0]), max(accellimitmaxsport, accel_limits[1]) ] # sport elif gasbuttonstatus == 2: accellimitmaxeco = -0.0015 * v_ego + 0.1 jerk_limits = [ min(-0.1, accel_limits[0] * 0.5), max(accellimitmaxeco, accel_limits[1]) ] # eco if not CS.carState.leftBlinker and not CS.carState.rightBlinker: steering_angle = CS.carState.steeringAngle if self.lastlat_Control and v_ego > 11: angle_later = self.lastlat_Control.anglelater else: angle_later = 0 else: angle_later = 0 steering_angle = 0 accel_limits = limit_accel_in_turns( v_ego, steering_angle, accel_limits, self.CP, angle_later * self.CP.steerRatio) if force_slow_decel: # if required so, force a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], _DT_MPC) # 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(CS.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.mpc1.update(CS, lead_1, v_cruise_setpoint) self.mpc2.update(CS, lead_2, v_cruise_setpoint) self.choose_solution(v_cruise_setpoint, enabled) # determine fcw if self.mpc1.new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo, lead_1.dRel, lead_1.vLead, lead_1.aLeadK, lead_1.yRel, lead_1.vLat, lead_1.fcw, blinkers) and not CS.carState.brakePressed if fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5 # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') # TODO: Move all these events to controlsd. This has nothing to do with planning events = [] if model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) radar_errors = list(live20.live20.radarErrors) if 'commIssue' in radar_errors: events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if 'fault' in radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) plan_send.plan.events = events plan_send.plan.mdMonoTime = md.logMonoTime plan_send.plan.l20MonoTime = live20.logMonoTime # longitudal plan plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.hasrightLaneDepart = bool( PP.r_poly[3] > -1.1 and not CS.carState.rightBlinker) plan_send.plan.hasleftLaneDepart = bool( PP.l_poly[3] < 1.05 and not CS.carState.leftBlinker) plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = v_curvature plan_send.plan.decelForTurn = decel_for_turn plan_send.plan.mapValid = map_valid # Send out fcw fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration dt = 0.05 # s a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
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 = 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.kegman = kegman_conf() self.mpc_frame = 0 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, 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 self.mpc_frame % 1000 == 0: self.kegman = kegman_conf() self.mpc_frame = 0 self.mpc_frame += 1 # 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 = 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.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, 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 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.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.kegman = KegmanConf() self.mpc_frame = 0 def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'model': self.v_model, '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 elif slowest == 'model': self.v_acc = self.v_model self.a_acc = self.a_model self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.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_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) if self.mpc_frame % 1000 == 0: self.kegman = KegmanConf() self.mpc_frame = 0 self.mpc_frame += 1 if len(sm['model'].path.poly) and int(self.kegman.conf['slowOnCurves']): 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 = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph 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 = MAX_SPEED # Calculate speed for normal cruise control if enabled: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] 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.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) 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 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_send.init('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
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.poller = zmq.Poller() self.arne182Status = messaging.sub_sock( service_list['arne182Status'].port, poller=self.poller, conflate=True) self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) 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() def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'model': self.v_model, '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 elif slowest == 'model': self.v_acc = self.v_model self.a_acc = self.a_model self.v_acc_future = min([ self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint ]) def update(self, sm, pm, CP, VM, PP): self.arne182 = None for socket, _ in self.poller.poll(0): if socket is self.arne182Status: self.arne182 = arne182.Arne182Status.from_bytes(socket.recv()) if self.arne182 is None: gasbuttonstatus = 0 else: gasbuttonstatus = self.arne182.gasbuttonstatus """Gets called when new radarState is available""" cur_time = sec_since_boot() v_ego = sm['carState'].vEgo if gasbuttonstatus == 1: speed_ahead_distance = 150 elif gasbuttonstatus == 2: speed_ahead_distance = 350 else: speed_ahead_distance = 250 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 < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 v_speedlimit = NO_CURVATURE_SPEED v_curvature_map = NO_CURVATURE_SPEED v_speedlimit_ahead = NO_CURVATURE_SPEED if len(sm['model'].path.poly): 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 = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph 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 = MAX_SPEED now = datetime.now() try: if sm['liveMapData'].speedLimitValid and osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: speed_limit = sm['liveMapData'].speedLimit v_speedlimit = speed_limit + offset else: speed_limit = None if sm['liveMapData'].speedLimitAheadValid and sm[ 'liveMapData'].speedLimitAheadDistance < speed_ahead_distance and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: 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 v_speedlimit_ahead = speed_limit_ahead + offset if sm['liveMapData'].curvatureValid and osm and ( sm['liveMapData'].lastGps.timestamp - time.mktime(now.timetuple()) * 1000) < 10000: curvature = abs(sm['liveMapData'].curvature) radius = 1 / max(1e-4, curvature) if radius > 500: c = 0.7 # 0.7 at 1000m = 95 kph elif radius > 250: c = 2.7 - 1 / 250 * radius # 1.7 at 264m 76 kph else: c = 3.0 - 13 / 2500 * 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.])) v_cruise_setpoint = min([ v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead ]) # Calculate speed for normal cruise control if enabled: accel_limits = [ float(x) for x in calc_cruise_accel_limits( v_ego, following, gasbuttonstatus) ] 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: 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 self.longitudinalPlanSource == 'cruise' and v_ego > v_speedlimit_ahead: 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) accel_limits[0] = required_decel accel_limits[1] = required_decel self.a_acc_start = required_decel 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.mpc1.update(pm, sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(pm, sm['carState'], lead_2, v_cruise_setpoint) 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 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_send.init('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 or v_speedlimit_ahead < min([v_speedlimit, v_ego + 1.])) 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 + (DT_PLAN / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
class Planner(): def __init__(self, CP): self.CP = CP self.mpcs = {} self.mpcs['lead0'] = LeadMpc(0) self.mpcs['lead1'] = LeadMpc(1) self.mpcs['cruise'] = LongitudinalMpc() self.fcw = False self.fcw_checker = FCWChecker() self.v_desired = 0.0 self.a_desired = 0.0 self.longitudinalPlanSource = 'cruise' self.alpha = np.exp(-DT_MDL / 2.0) self.lead_0 = log.ModelDataV2.LeadDataV3.new_message() self.lead_1 = log.ModelDataV2.LeadDataV3.new_message() self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) # dp self.dp_accel_profile_ctrl = False self.dp_accel_profile = DP_ACCEL_ECO self.dp_following_profile_ctrl = False self.dp_following_profile = 3 self.dp_following_dist = 1.8 # default val def update(self, sm, CP): # dp self.dp_accel_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl self.dp_accel_profile = sm['dragonConf'].dpAccelProfile self.dp_following_profile_ctrl = sm['dragonConf'].dpAccelProfileCtrl self.dp_following_profile = sm['dragonConf'].dpFollowingProfile self.dp_following_dist = DP_FOLLOWING_DIST[ 0 if not self.dp_following_profile_ctrl else self. dp_following_profile] self.mpcs['lead0'].set_following_distance(self.dp_following_dist) self.mpcs['lead1'].set_following_distance(self.dp_following_dist) cur_time = sec_since_boot() v_ego = sm['carState'].vEgo a_ego = sm['carState'].aEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel self.lead_0 = sm['radarState'].leadOne self.lead_1 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if not enabled or sm['carState'].gasPressed: self.v_desired = v_ego self.a_desired = a_ego # Prevent divergence, smooth in current v_ego self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego self.v_desired = max(0.0, self.v_desired) if not self.dp_accel_profile_ctrl: accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] else: accel_limits = dp_calc_cruise_accel_limits(v_cruise, self.dp_accel_profile) 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]) # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired) self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) next_a = np.inf for key in self.mpcs: self.mpcs[key].set_cur_state(self.v_desired, self.a_desired) self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise) if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: self.longitudinalPlanSource = key self.v_desired_trajectory = self.mpcs[ key].v_solution[:CONTROL_N] self.a_desired_trajectory = self.mpcs[ key].a_solution[:CONTROL_N] self.j_desired_trajectory = self.mpcs[ key].j_solution[:CONTROL_N] next_a = self.mpcs[key].a_solution[5] # determine fcw if self.mpcs['lead0'].new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update( self.mpcs['lead0'].mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.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_prev = self.a_desired self.a_desired = float( interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory] longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] longitudinalPlan.hasLead = self.mpcs['lead0'].status longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource longitudinalPlan.fcw = self.fcw pm.send('longitudinalPlan', plan_send)
class Planner(object): def __init__(self, CP, fcw_enabled): self.CP = CP self.poller = zmq.Poller() self.plan = messaging.pub_sock(service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) 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.fcw_enabled = fcw_enabled self.params = Params() 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, VM, PP, live_map_data): """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_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 v_speedlimit = NO_CURVATURE_SPEED v_curvature = NO_CURVATURE_SPEED #map_age = cur_time - rcv_times['liveMapData'] map_valid = False # live_map_data.liveMapData.mapValid and map_age < 10.0 # Speed limit and curvature set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active and map_valid: if live_map_data.liveMapData.speedLimitValid: speed_limit = live_map_data.liveMapData.speedLimit offset = float(self.params.get("SpeedLimitOffset")) v_speedlimit = speed_limit + offset if live_map_data.liveMapData.curvatureValid: curvature = abs(live_map_data.liveMapData.curvature) a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = math.sqrt(a_y_max / max(1e-4, curvature)) v_curvature = min(NO_CURVATURE_SPEED, v_curvature) decel_for_turn = bool( v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.])) v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit]) # Calculate speed for normal cruise control if enabled: 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 = 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[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) # Change accel limits based on time remaining to turn if decel_for_turn: time_to_turn = max( 1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.)) required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn) accel_limits[0] = max(accel_limits[0], required_decel) self.v_cruise, self.a_cruise = speed_smoother( self.v_acc_start, self.a_acc_start, v_cruise_setpoint, accel_limits[1], accel_limits[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.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint) 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 fcw = self.fcw_checker.update( self.mpc1.mpc_solution, cur_time, 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_send.init('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 = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.vCurvature = v_curvature plan_send.plan.decelForTurn = decel_for_turn plan_send.plan.mapValid = map_valid 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 fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * ( self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
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
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.mpcs = {} self.mpcs['lead0'] = LeadMpc(0) self.mpcs['lead1'] = LeadMpc(1) self.mpcs['cruise'] = LongitudinalMpc() self.fcw = False self.fcw_checker = FCWChecker() self.v_desired = 0.0 self.a_desired = 0.0 self.longitudinalPlanSource = 'cruise' self.alpha = np.exp(-DT_MDL / 2.0) self.lead_0 = log.ModelDataV2.LeadDataV3.new_message() self.lead_1 = log.ModelDataV2.LeadDataV3.new_message() self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.params = Params() self.target_speed_map = 0 self.target_speed_map_dist = 0 self.target_speed_map_dist_prev = 0 self.target_speed_map_block = False self.target_speed_map_sign = False self.map_sign = 0 self.vego = 0 self.second = 0 self.map_enabled = False def update(self, sm, CP): cur_time = sec_since_boot() v_ego = sm['carState'].vEgo a_ego = sm['carState'].aEgo self.vego = v_ego # if sm['controlsState'].mapSign == 124: # v_cruise_kph = 20. if CP.sccBus != 0: v_cruise_kph = sm['carState'].vSetDis else: v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel self.lead_0 = sm['radarState'].leadOne self.lead_1 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) if not enabled or sm['carState'].gasPressed: self.v_desired = v_ego self.a_desired = a_ego # Prevent divergence, smooth in current v_ego self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego self.v_desired = max(0.0, self.v_desired) accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] 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]) # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired) self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) next_a = np.inf for key in self.mpcs: self.mpcs[key].set_cur_state(self.v_desired, self.a_desired) self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise) if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: self.longitudinalPlanSource = key self.v_desired_trajectory = self.mpcs[ key].v_solution[:CONTROL_N] self.a_desired_trajectory = self.mpcs[ key].a_solution[:CONTROL_N] self.j_desired_trajectory = self.mpcs[ key].j_solution[:CONTROL_N] next_a = self.mpcs[key].a_solution[5] # determine fcw if self.mpcs['lead0'].new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update( self.mpcs['lead0'].mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.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_prev = self.a_desired self.a_desired = float( interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0 # opkr self.second += 1 if self.second > 30: self.map_enabled = self.params.get_bool("OpkrMapEnable") self.second = 0 if self.map_enabled and v_ego > 0.3: self.map_sign = sm['liveMapData'].safetySign self.target_speed_map_dist = sm['liveMapData'].speedLimitDistance if self.target_speed_map_dist_prev != self.target_speed_map_dist: self.target_speed_map_dist_prev = self.target_speed_map_dist self.target_speed_map = sm['liveMapData'].speedLimit if self.target_speed_map > 29: if self.target_speed_map_dist > 1250: self.target_speed_map_block = True else: self.target_speed_map_block = False def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_alive_and_valid( service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory] longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] longitudinalPlan.hasLead = self.mpcs['lead0'].status longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource longitudinalPlan.fcw = self.fcw # opkr # 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) longitudinalPlan.dynamicTRMode = int( self.mpcs['lead0'].dynamic_TR_mode) longitudinalPlan.dynamicTRValue = float(self.mpcs['lead0'].dynamic_TR) if self.map_enabled: longitudinalPlan.mapSign = float(self.map_sign) cam_distance_calc = 0 cam_distance_calc = interp(self.vego * CV.MS_TO_KPH, [30, 110], [2.8, 4.5]) # 감속 기본 거리 consider_speed = interp( (self.vego * CV.MS_TO_KPH - self.target_speed_map), [0, 40], [1, 2]) # 속도차에 따른 거리 추가 if 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) longitudinalPlan.onSpeedControl = True elif 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) longitudinalPlan.onSpeedControl = True 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) longitudinalPlan.onSpeedControl = True self.target_speed_map_sign = True elif self.target_speed_map > 29 and self.target_speed_map_dist < 600.: longitudinalPlan.targetSpeedCamera = float( self.target_speed_map) longitudinalPlan.targetSpeedCameraDist = float( self.target_speed_map_dist) longitudinalPlan.onSpeedControl = False elif self.target_speed_map == 0 and self.target_speed_map_dist != 0.: longitudinalPlan.targetSpeedCamera = float( self.target_speed_map) longitudinalPlan.targetSpeedCameraDist = float( self.target_speed_map_dist) longitudinalPlan.onSpeedControl = False self.target_speed_map_sign = False else: longitudinalPlan.targetSpeedCamera = 0 longitudinalPlan.targetSpeedCameraDist = 0 longitudinalPlan.onSpeedControl = False self.target_speed_map_sign = False 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 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() 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: 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) 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 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.mpcs = {} self.mpcs['lead0'] = LeadMpc(0) self.mpcs['lead1'] = LeadMpc(1) self.mpcs['cruise'] = LongitudinalMpc() self.mpcs['custom'] = LimitsLongitudinalMpc() self.fcw = False self.fcw_checker = FCWChecker() self.v_desired = 0.0 self.a_desired = 0.0 self.longitudinalPlanSource = 'cruise' self.alpha = np.exp(-CP.radarTimeStep/2.0) self.lead_0 = log.ModelDataV2.LeadDataV3.new_message() self.lead_1 = log.ModelDataV2.LeadDataV3.new_message() self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.vision_turn_controller = VisionTurnController(CP) self.speed_limit_controller = SpeedLimitController() self.events = Events() self.turn_speed_controller = TurnSpeedController() self._params = Params() self.params_check_last_t = 0. self.params_check_freq = 0.1 # check params at 10Hz self.accel_mode = int(self._params.get("AccelMode", encoding="utf8")) # 0 = normal, 1 = sport; 2 = eco; 3 = creep self.coasting_lead_d = -1. # [m] lead distance. -1. if no lead self.coasting_lead_v = -10. # lead "absolute"" velocity self.tr = 1.8 self.sessionInitTime = sec_since_boot() self.debug_logging = False self.debug_log_time_step = 0.333 self.last_debug_log_t = 0. self.debug_log_path = "/data/openpilot/long_debug.csv" if self.debug_logging: with open(self.debug_log_path,"w") as f: f.write(",".join([ "t", "vEgo", "vEgo (mph)", "a lim low", "a lim high", "out a", "out long plan"]) + "\n") def update(self, sm, CP): cur_time = sec_since_boot() t = cur_time v_ego = sm['carState'].vEgo a_ego = sm['carState'].aEgo v_cruise_kph = sm['controlsState'].vCruise v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_state = sm['controlsState'].longControlState force_slow_decel = sm['controlsState'].forceDecel self.lead_0 = sm['radarState'].leadOne self.lead_1 = sm['radarState'].leadTwo enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0 if self.lead_1.status: self.coasting_lead_d = self.lead_1.dRel self.coasting_lead_v = self.lead_1.vLead else: self.coasting_lead_d = -1. self.coasting_lead_v = -10. self.tr = self.mpcs['lead0'].tr if not enabled or sm['carState'].gasPressed: self.v_desired = v_ego self.a_desired = a_ego # Prevent divergence, smooth in current v_ego self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego self.v_desired = max(0.0, self.v_desired) # Get acceleration and active solutions for custom long mpc. a_mpc, active_mpc, c_source = self.mpc_solutions(enabled, self.v_desired, self.a_desired, v_cruise, sm) if t - self.params_check_last_t >= self.params_check_freq: self.params_check_last_t = t accel_mode = int(self._params.get("AccelMode", encoding="utf8")) # 0 = normal, 1 = sport; 2 = eco if accel_mode != self.accel_mode: cloudlog.info(f"Acceleration mode changed, new value: {accel_mode} = {['normal','sport','eco','creep'][accel_mode]}") self.accel_mode = accel_mode accel_limits = calc_cruise_accel_limits(v_ego, following, self.accel_mode) 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]) # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired) self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) # ensure lower accel limit (for braking) is lower than target acc for custom controllers. accel_limits = [min(accel_limits_turns[0], a_mpc['custom']), accel_limits_turns[1]] self.mpcs['custom'].set_accel_limits(accel_limits[0], accel_limits[1]) next_a = np.inf for key in self.mpcs: self.mpcs[key].set_cur_state(self.v_desired, self.a_desired) self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise, a_mpc[key], active_mpc[key]) # picks slowest solution from accel in ~0.2 seconds if self.mpcs[key].status and active_mpc[key] and self.mpcs[key].a_solution[5] < next_a: self.longitudinalPlanSource = c_source if key == 'custom' else key self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N] self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N] self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N] next_a = self.mpcs[key].a_solution[5] # debug logging do_log = self.debug_logging and (t - self.last_debug_log_t > self.debug_log_time_step) if do_log: self.last_debug_log_t = t f = open(self.debug_log_path,"a") f.write(",".join([f"{i:.1f}" if i == float else str(i) for i in [ t - self.sessionInitTime, v_ego, v_ego * CV.MS_TO_MPH, accel_limits[0], accel_limits[1], next_a, self.longitudinalPlanSource]]) + "\n") f.close() # determine fcw if self.mpcs['lead0'].new_lead: self.fcw_checker.reset_lead(cur_time) blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker self.fcw = self.fcw_checker.update(self.mpcs['lead0'].mpc_solution, cur_time, sm['controlsState'].active, v_ego, sm['carState'].aEgo, self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK, self.lead_1.yRel, self.lead_1.vLat, self.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_prev = self.a_desired self.a_desired = float(interp(CP.radarTimeStep, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired = self.v_desired + CP.radarTimeStep * (self.a_desired + a_prev)/2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory] longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory] longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory] longitudinalPlan.hasLead = self.mpcs['lead0'].status longitudinalPlan.leadDist = self.coasting_lead_d longitudinalPlan.leadV = self.coasting_lead_v longitudinalPlan.desiredFollowDistance = self.mpcs['lead0'].tr longitudinalPlan.leadDistCost = self.mpcs['lead0'].dist_cost longitudinalPlan.leadAccelCost = self.mpcs['lead0'].accel_cost longitudinalPlan.stoppingDistance = self.mpcs['lead0'].stopping_distance longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource longitudinalPlan.fcw = self.fcw longitudinalPlan.visionTurnControllerState = self.vision_turn_controller.state longitudinalPlan.visionTurnSpeed = float(self.vision_turn_controller.v_turn) longitudinalPlan.speedLimitControlState = self.speed_limit_controller.state longitudinalPlan.speedLimit = float(self.speed_limit_controller.speed_limit) longitudinalPlan.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset) longitudinalPlan.distToSpeedLimit = float(self.speed_limit_controller.distance) longitudinalPlan.isMapSpeedLimit = bool(self.speed_limit_controller.source == SpeedLimitResolver.Source.map_data) longitudinalPlan.eventsDEPRECATED = self.events.to_msg() longitudinalPlan.turnSpeedControlState = self.turn_speed_controller.state longitudinalPlan.turnSpeed = float(self.turn_speed_controller.speed_limit) longitudinalPlan.distToTurn = float(self.turn_speed_controller.distance) longitudinalPlan.turnSign = int(self.turn_speed_controller.turn_sign) pm.send('longitudinalPlan', plan_send) def mpc_solutions(self, enabled, v_ego, a_ego, v_cruise, sm): # Update controllers self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm) self.events = Events() self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.events) self.turn_speed_controller.update(enabled, v_ego, a_ego, sm) # Pick solution with lowest acceleration target. a_solutions = {None: float("inf")} if self.vision_turn_controller.is_active: a_solutions['turn'] = self.vision_turn_controller.a_target if self.speed_limit_controller.is_active: a_solutions['limit'] = self.speed_limit_controller.a_target if self.turn_speed_controller.is_active: a_solutions['turnlimit'] = self.turn_speed_controller.a_target source = min(a_solutions, key=a_solutions.get) a_sol = { 'cruise': a_ego, # Irrelevant 'lead0': a_ego, # Irrelevant 'lead1': a_ego, # Irrelevant 'custom': 0. if source is None else a_solutions[source], } active_sol = { 'cruise': True, # Irrelevant 'lead0': True, # Irrelevant 'lead1': True, # Irrelevant 'custom': source is not None, } return a_sol, active_sol, source
class Planner(object): def __init__(self, CP, fcw_enabled): self.CP = CP self.plan = messaging.pub_sock(service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock(service_list['liveLongitudinalMpc'].port) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) 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.fcw_enabled = fcw_enabled self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K) self.model_v_kf_ready = False self.params = Params() def choose_solution(self, v_cruise_setpoint, enabled): if enabled: solutions = {'cruise': self.v_cruise, 'model': 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 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.v_model self.a_acc = self.a_model self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) 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_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 not self.model_v_kf_ready: self.model_v_kf.x = [[v_ego],[0.0]] self.model_v_kf_ready = True if len(sm['model'].speed): self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX]) if self.params.get("LimitSetSpeedNeural") == "1": model_speed = self.model_v_kf.x[0][0] else: model_speed = MAX_SPEED # Calculate speed for normal cruise control if enabled: 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) # accel and jerk up limits are higher here to make model not limiting accel # mainly done to prevent flickering of slowdown icon self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start, model_speed, 2*accel_limits[1], 3*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.mpc1.update(sm['carState'], lead_1, v_cruise_setpoint) self.mpc2.update(sm['carState'], lead_2, v_cruise_setpoint) 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 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_send.init('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 = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vStart = self.v_acc_start plan_send.plan.aStart = self.a_acc_start plan_send.plan.vTarget = self.v_acc plan_send.plan.aTarget = self.a_acc plan_send.plan.vTargetFuture = 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 fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) # Interpolate 0.05 seconds and save as starting point for next iteration a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol
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.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 = 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): 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, 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')) self.offset = 0 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 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 speed_ahead_distance = default_brake_distance v_speedlimit = NO_CURVATURE_SPEED v_curvature_map = NO_CURVATURE_SPEED v_speedlimit_ahead = 0 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 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 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) ] 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_ego * 3.6) <= 50: if sm['liveMapData'].speedLimitAhead and sm[ 'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 * 2.5): v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead elif (v_ego * 3.6) <= 70: if sm['liveMapData'].speedLimitAhead and sm[ 'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 * 3.5): v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead elif (v_ego * 3.6) > 70: if sm['liveMapData'].speedLimitAhead and sm[ 'liveMapData'].speedLimitAheadDistance < (v_ego * 3.6 * 4.5): v_speedlimit_ahead = sm['liveMapData'].speedLimitAhead #v_cruise_setpoint = min([v_cruise_setpoint, v_curvature_map, v_speedlimit, v_speedlimit_ahead]) v_cruise_setpoint = min( [v_cruise_setpoint, v_curvature_map, v_speedlimit]) 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(pm, sm['carState'], lead_1) self.mpc2.update(pm, 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 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 # 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
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 self.input_queue = Queue() self.input_thread = Thread(target=input_loop, args=(self.input_queue, )) self.input_thread.start() self.output_queue = Queue() self.output_thread = Thread(target=output_loop, args=(self.output_queue, )) self.output_thread.start() self.TR_override = None 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() try: input_event = self.input_queue.get_nowait() if input_event == InputEvent.LONG_PRESS: self.TR_override = 1.8 if self.TR_override is None else None # Output current status output_event = OutputEvent.LONG_DIM if self.TR_override is None else OutputEvent.SHORT_DIM try: self.output_queue.put_nowait(output_event) except Full: log_f = open('/data/openpilot-patch/mpc_update_log.txt', 'a') log_f.write(f"{datetime.now()} Output queue is full\n") log_f.flush() subprocess.Popen( ['python', '/data/openpilot-patch/util/error.py']) except TimeoutError: pass except Empty: pass 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.TR_override) self.mpc2.update(sm['carState'], lead_2, self.TR_override) 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)