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 __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 __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 __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.first_loop = True # dp self.dp_profile = DP_OFF # dp - slow on curve from 0.7.6.1 self.dp_slow_on_curve = False self.v_model = 0.0 self.a_model = 0.0
def __init__(self, CP): self.CP = CP self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc_next = 0.0 self.a_acc_next = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.fcw = False self.params = Params() self.first_loop = True
def __init__(self, CP): self.CP = CP self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) if not travis: pm = messaging.PubMaster(['smiskolData']) # why is this here? self.mpc1.set_pm(pm) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.params = Params() self.first_loop = True
def __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 __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.first_loop = True # dp self.dragon_slow_on_curve = True self.dragon_alt_accel_profile = False self.dragon_fast_accel = False self.dragon_accel_profile = AP_OFF self.last_ts = 0. self.dp_last_modified = None
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 __init__(self, CP): self.CP = CP self.op_params = opParams() self.slowdown_for_curves = self.op_params.get('slowdown_for_curves') self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.mpc_model = LongitudinalMpcModel() self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.params = Params() self.first_loop = True
def __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.path_x = np.arange(192) self.params = Params()
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 __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 __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 run_following_distance_simulation(v_lead, t_end=200.0): dt = 0.2 t = 0. x_lead = 200.0 x_ego = 0.0 v_ego = v_lead a_ego = 0.0 v_cruise_setpoint = v_lead + 10. pm = FakePubMaster() mpc = LongitudinalMpc(1) first = True while t < t_end: # Run cruise control accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, False) ] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], dt) # Setup CarState CS = messaging.new_message('carState') CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego # Setup lead packet lead = log.RadarState.LeadData.new_message() lead.status = True lead.dRel = x_lead - x_ego lead.vLead = v_lead lead.aLeadK = 0.0 # Run MPC mpc.set_cur_state(v_ego, a_ego) if first: # Make sure MPC is converged on first timestep for _ in range(20): mpc.update(CS.carState, lead) mpc.publish(pm) mpc.update(CS.carState, lead) mpc.publish(pm) # Choose slowest of two solutions if v_cruise < mpc.v_mpc: v_ego, a_ego = v_cruise, a_cruise else: v_ego, a_ego = mpc.v_mpc, mpc.a_mpc # Update state x_lead += v_lead * dt x_ego += v_ego * dt t += dt first = False return x_lead - x_ego
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 __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 = kegman_conf()
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 __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 __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)
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.use_cluster_speed = Params().get_bool('UseClusterSpeed') self.long_control_enabled = Params().get_bool('LongControlEnabled')
def run_following_distance_simulation(TR_override, v_lead, t_end=200.0): dt = 0.2 t = 0. lead_pos = LeadPos(v_lead) x_ego = 0.0 v_ego = v_lead a_ego = 0.0 v_cruise_setpoint = v_lead + 10. pm = FakePubMaster() mpc = LongitudinalMpc(1, TR_override) datapoints = [{'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}] first = True while t < t_end: # Run cruise control accel_limits = [ float(x) for x in calc_cruise_accel_limits(v_ego, False) ] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint, accel_limits[1], accel_limits[0], jerk_limits[1], jerk_limits[0], dt) # Setup CarState CS = messaging.new_message('carState') CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego # Setup lead packet lead = log.RadarState.LeadData.new_message() lead.status = True lead.dRel = lead_pos.x - x_ego lead.vLead = lead_pos.v lead.aLeadK = 0.0 # Run MPC mpc.set_cur_state(v_ego, a_ego) if first: # Make sure MPC is converged on first timestep for _ in range(20): mpc.update(CS.carState, lead) mpc.publish(pm) mpc.update(CS.carState, lead) mpc.publish(pm) # Choose slowest of two solutions if v_cruise < mpc.v_mpc: v_ego, a_ego = v_cruise, a_cruise else: v_ego, a_ego = mpc.v_mpc, mpc.a_mpc # Update state lead_pos.update(t, dt) x_ego += v_ego * dt t += dt first = False datapoints.append({'t': t, 'x_ego': x_ego, 'x_lead': lead_pos.x}) filename = f'test_out/{v_lead}_{TR_override}.json' with open(filename, 'w') as datafile: json.dump(datapoints, datafile) return lead_pos.x - x_ego