def __init__(self, CP): self.MP = ModelParser() self.last_cloudlog_t = 0 context = zmq.Context() self.plan = messaging.pub_sock(context, service_list['pathPlan'].port) self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.invalid_counter = 0
def __init__(self, CP): self.MP = ModelParser() self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] self.last_cloudlog_t = 0 self.plan = messaging.pub_sock(service_list['pathPlan'].port) self.livempc = messaging.pub_sock(service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0
def __init__(self, VM, mocked): self.VM = VM self.mocked = mocked self.MP = ModelParser() self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 self.steer_angle = 0. self.steer_override = False # Kalman filter stuff: self.ekfv = EKFV1D() self.speedSensorV = SimpleSensor(XV, 1, 2) # v_ego self.v_ego = 0. self.v_ego_hist_t = deque([0], maxlen=v_len) self.v_ego_hist_v = deque([0], maxlen=v_len) self.v_ego_t_aligned = 0.
class PathPlanner(object): def __init__(self, CP): self.MP = ModelParser() self.last_cloudlog_t = 0 context = zmq.Context() self.plan = messaging.pub_sock(context, service_list['pathPlan'].port) self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.invalid_counter = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") self.cur_state[0].x = 0.0 self.cur_state[0].y = 0.0 self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 def update(self, CP, VM, CS, md, live100, live_parameters): v_ego = CS.carState.vEgo angle_steers = CS.carState.steeringAngle active = live100.live100.active angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage self.MP.update(v_ego, md) # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio) curvature_factor = VM.curvature_factor(v_ego) l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly)) r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly)) p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly)) # account for actuation delay self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, l_poly, r_poly, p_poly, self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float( math.degrees(delta_desired * VM.sR) + angle_offset_bias) # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.invalid_counter += 1 else: self.invalid_counter = 0 plan_valid = self.invalid_counter < 2 plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.pathPlan.laneWidth = float(self.MP.lane_width) plan_send.pathPlan.dPoly = map(float, self.MP.d_poly) plan_send.pathPlan.cPoly = map(float, self.MP.c_poly) plan_send.pathPlan.cProb = float(self.MP.c_prob) plan_send.pathPlan.lPoly = map(float, l_poly) plan_send.pathPlan.lProb = float(self.MP.l_prob) plan_send.pathPlan.rPoly = map(float, r_poly) plan_send.pathPlan.rProb = float(self.MP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = float( live_parameters.liveParameters.angleOffsetAverage) plan_send.pathPlan.valid = bool(plan_valid) plan_send.pathPlan.paramsValid = bool( live_parameters.liveParameters.valid) self.plan.send(plan_send.to_bytes()) dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost self.livempc.send(dat.to_bytes())
def radard_thread(gctx=None): set_realtime_priority(2) # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) mocked = CP.carName == "mock" or CP.carName == "tesla" or CP.carName == "hyundai" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface context = zmq.Context() # *** subscribe to features and model from visiond poller = zmq.Poller() model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller) # Default parameters live_parameters = messaging.new_message() live_parameters.init('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 MP = ModelParser() RI = RadarInterface(CP) last_md_ts = 0 last_l100_ts = 0 # *** publish live20 and liveTracks live20 = messaging.pub_sock(context, service_list['live20'].port) liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port) path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max # Time-alignment rate = 20. # model and radar are both at 20Hz tsv = 1./rate v_len = 20 # how many speed data points to remember for t alignment with rdr data active = 0 steer_angle = 0. steer_override = False tracks = defaultdict(dict) # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) # v_ego v_ego = None v_ego_hist_t = deque(maxlen=v_len) v_ego_hist_v = deque(maxlen=v_len) v_ego_t_aligned = 0. rk = Ratekeeper(rate, print_delay_threshold=np.inf) while 1: rr = RI.update() ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured] # receive the live100s l100 = None md = None for socket, event in poller.poll(0): if socket is live100: l100 = messaging.recv_one(socket) elif socket is model: md = messaging.recv_one(socket) elif socket is live_parameters_sock: live_parameters = messaging.recv_one(socket) VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio) if l100 is not None: active = l100.live100.active v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers steer_override = l100.live100.steerOverride v_ego_hist_v.append(v_ego) v_ego_hist_t.append(float(rk.frame)/rate) last_l100_ts = l100.logMonoTime if v_ego is None: continue if md is not None: last_md_ts = md.logMonoTime # *** get path prediction from the model *** MP.update(v_ego, md) # run kalman filter only if prob is high enough if MP.lead_prob > 0.7: reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var)) ekfv.update_scalar(reading) ekfv.predict(tsv) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0: ekfv.state[XV] = MP.lead_dist ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) else: ekfv.state[XV] = MP.lead_dist ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init])) ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] # *** compute the likely path_y *** if (active and not steer_override) or mocked: # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(MP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0] # *** remove missing points from meta data *** for ids in tracks.keys(): if ids not in ar_pts: tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: # ignore standalone vision point, unless we are mocking the radar if ids == VISION_POINT and not mocked: continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement cur_time = float(rk.frame)/rate v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) # create the track if it doesn't exist or it's a new track if ids not in tracks: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned, rpt[3], steer_override) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: fused_id = None best_score = NO_FUSION_SCORE for ids in tracks: dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - tracks[ids].dRel)) ** 2 + (2*(ar_pts[VISION_POINT][1] - tracks[ids].yRel)) ** 2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - tracks[ids].vRel) tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) if best_score > tracks[ids].vision_score: fused_id = ids best_score = tracks[ids].vision_score if fused_id is not None: tracks[fused_id].vision_cnt += 1 tracks[fused_id].update_vision_fusion() if DEBUG: print("NEW CYCLE") if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) idens = tracks.keys() track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: link = linkage_vector(track_pts, method='centroid') cluster_idxs = fcluster(link, 2.5, criterion='distance') clusters = [None]*max(cluster_idxs) for idx in xrange(len(track_pts)): cluster_i = cluster_idxs[idx]-1 if clusters[cluster_i] == None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(tracks[idens[idx]]) elif len(track_pts) == 1: # TODO: why do we need this? clusters = [Cluster()] clusters[0].add(tracks[idens[0]]) else: clusters = [] if DEBUG: for i in clusters: print(i) # *** extract the lead car *** lead_clusters = [c for c in clusters if c.is_potential_lead(v_ego)] lead_clusters.sort(key=lambda x: x.dRel) lead_len = len(lead_clusters) # *** extract the second lead from the whole set of leads *** lead2_clusters = [c for c in lead_clusters if c.is_potential_lead2(lead_clusters)] lead2_clusters.sort(key=lambda x: x.dRel) lead2_len = len(lead2_clusters) # *** publish live20 *** dat = messaging.new_message() dat.init('live20') dat.live20.mdMonoTime = last_md_ts dat.live20.canMonoTimes = list(rr.canMonoTimes) dat.live20.radarErrors = list(rr.errors) dat.live20.l100MonoTime = last_l100_ts if lead_len > 0: dat.live20.leadOne = lead_clusters[0].toLive20() if lead2_len > 0: dat.live20.leadTwo = lead2_clusters[0].toLive20() else: dat.live20.leadTwo.status = False else: dat.live20.leadOne.status = False dat.live20.cumLagMs = -rk.remaining*1000. live20.send(dat.to_bytes()) # *** publish tracks for UI debugging (keep last) *** dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): if DEBUG: print("id: %4.0f x: %4.1f y: %4.1f vr: %4.1f d: %4.1f va: %4.1f vl: %4.1f vlk: %4.1f alk: %4.1f s: %1.0f v: %1.0f" % \ (ids, tracks[ids].dRel, tracks[ids].yRel, tracks[ids].vRel, tracks[ids].dPath, tracks[ids].vLat, tracks[ids].vLead, tracks[ids].vLeadK, tracks[ids].aLeadK, tracks[ids].stationary, tracks[ids].measured)) dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), "yRel": float(tracks[ids].yRel), "vRel": float(tracks[ids].vRel), "aRel": float(tracks[ids].aRel), "stationary": bool(tracks[ids].stationary), "oncoming": bool(tracks[ids].oncoming), } liveTracks.send(dat.to_bytes()) rk.monitor_time()
class RadarD(object): def __init__(self, VM, mocked): self.VM = VM self.mocked = mocked self.MP = ModelParser() self.tracks = defaultdict(dict) self.last_md_ts = 0 self.last_controls_state_ts = 0 self.active = 0 self.steer_angle = 0. self.steer_override = False # Kalman filter stuff: self.ekfv = EKFV1D() self.speedSensorV = SimpleSensor(XV, 1, 2) # v_ego self.v_ego = 0. self.v_ego_hist_t = deque([0], maxlen=v_len) self.v_ego_hist_v = deque([0], maxlen=v_len) self.v_ego_t_aligned = 0. def update(self, frame, delay, sm, rr): ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [ pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured ] if sm.updated['liveParameters']: self.VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.steer_angle = sm['controlsState'].angleSteers self.steer_override = sm['controlsState'].steerOverride self.v_ego_hist_v.append(self.v_ego) self.v_ego_hist_t.append(float(frame) / rate) self.last_controls_state_ts = sm.logMonoTime['controlsState'] if sm.updated['model']: self.last_md_ts = sm.logMonoTime['model'] self.MP.update(self.v_ego, sm['model']) # run kalman filter only if prob is high enough if self.MP.lead_prob > 0.7: reading = self.speedSensorV.read(self.MP.lead_dist, covar=np.matrix(self.MP.lead_var)) self.ekfv.update_scalar(reading) self.ekfv.predict(DT_MDL) # When changing lanes the distance to the lead car can suddenly change, # which makes the Kalman filter output large relative acceleration if self.mocked and abs(self.MP.lead_dist - self.ekfv.state[XV]) > 2.0: self.ekfv.state[XV] = self.MP.lead_dist self.ekfv.covar = (np.diag( [self.MP.lead_var, self.ekfv.var_init])) self.ekfv.state[SPEEDV] = 0. ar_pts[VISION_POINT] = (float(self.ekfv.state[XV]), np.polyval(self.MP.d_poly, float(self.ekfv.state[XV])), float(self.ekfv.state[SPEEDV]), False) else: self.ekfv.state[XV] = self.MP.lead_dist self.ekfv.covar = (np.diag([self.MP.lead_var, self.ekfv.var_init])) self.ekfv.state[SPEEDV] = 0. if VISION_POINT in ar_pts: del ar_pts[VISION_POINT] # *** compute the likely path_y *** if (self.active and not self.steer_override) or self.mocked: # use path from model (always when mocking as steering is too noisy) path_y = np.polyval(self.MP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 it does not only report the physical offset path_y = calc_lookahead_offset( self.v_ego, self.steer_angle, path_x, self.VM, angle_offset=sm['liveParameters'].angleOffsetAverage)[0] # *** remove missing points from meta data *** for ids in self.tracks.keys(): if ids not in ar_pts: self.tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: # ignore standalone vision point, unless we are mocking the radar if ids == VISION_POINT and not self.mocked: continue rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement cur_time = float(frame) / rate self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v) d_path = np.sqrt( np.amin((path_x - rpt[0])**2 + (path_y - rpt[1])**2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y)) # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track() self.tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, self.v_ego_t_aligned, rpt[3], self.steer_override) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: fused_id = None best_score = NO_FUSION_SCORE for ids in self.tracks: dist_to_vision = np.sqrt( (0.5 * (ar_pts[VISION_POINT][0] - self.tracks[ids].dRel))**2 + (2 * (ar_pts[VISION_POINT][1] - self.tracks[ids].yRel))**2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - self.tracks[ids].vRel) self.tracks[ids].update_vision_score(dist_to_vision, rel_speed_diff) if best_score > self.tracks[ids].vision_score: fused_id = ids best_score = self.tracks[ids].vision_score if fused_id is not None: self.tracks[fused_id].vision_cnt += 1 self.tracks[fused_id].update_vision_fusion() if DEBUG: print("NEW CYCLE") if VISION_POINT in ar_pts: print("vision", ar_pts[VISION_POINT]) idens = list(self.tracks.keys()) track_pts = np.array( [self.tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them if len(track_pts) > 1: cluster_idxs = cluster_points_centroid(track_pts, 2.5) clusters = [None] * (max(cluster_idxs) + 1) for idx in xrange(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() clusters[cluster_i].add(self.tracks[idens[idx]]) elif len(track_pts) == 1: # TODO: why do we need this? clusters = [Cluster()] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] if DEBUG: for i in clusters: print(i) # *** extract the lead car *** lead_clusters = [ c for c in clusters if c.is_potential_lead(self.v_ego) ] lead_clusters.sort(key=lambda x: x.dRel) lead_len = len(lead_clusters) # *** extract the second lead from the whole set of leads *** lead2_clusters = [ c for c in lead_clusters if c.is_potential_lead2(lead_clusters) ] lead2_clusters.sort(key=lambda x: x.dRel) lead2_len = len(lead2_clusters) # *** publish radarState *** dat = messaging.new_message() dat.init('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState']) dat.radarState.mdMonoTime = self.last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) dat.radarState.radarErrors = list(rr.errors) dat.radarState.controlsStateMonoTime = self.last_controls_state_ts if lead_len > 0: dat.radarState.leadOne = lead_clusters[0].toRadarState() if lead2_len > 0: dat.radarState.leadTwo = lead2_clusters[0].toRadarState() else: dat.radarState.leadTwo.status = False else: dat.radarState.leadOne.status = False return dat
class PathPlanner(object): def __init__(self, CP): self.MP = ModelParser() self.l_poly = libmpc_py.ffi.new("double[4]") self.r_poly = libmpc_py.ffi.new("double[4]") self.p_poly = libmpc_py.ffi.new("double[4]") self.last_cloudlog_t = 0 self.plan = messaging.pub_sock(service_list['pathPlan'].port) self.livempc = messaging.pub_sock(service_list['liveMpc'].port) self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc #self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) self.libmpc.init(MPC_COST_LAT.PATH * 0.1, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") self.cur_state[0].x = 0.0 self.cur_state[0].y = 0.0 self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 self.mpc_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.mpc_rates = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.mpc_times = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 self.rate_des_prev = 0.0 self.angle_offset = 0.0 def update(self, sm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle cur_time = sec_since_boot() angle_offset_average = sm['liveParameters'].angleOffsetAverage max_offset_change = 0.001 / (abs(self.angle_offset) + 0.0001) self.angle_offset = clip( angle_offset_average + sm['controlsState'].lateralControlState.pidState.angleBias, self.angle_offset - max_offset_change, self.angle_offset + max_offset_change) self.MP.update(v_ego, sm['model']) # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) self.l_poly = list(self.MP.l_poly) self.r_poly = list(self.MP.r_poly) self.p_poly = list(self.MP.p_poly) # prevent over-inflation of desired angle actual_delta = math.radians(angle_steers - self.angle_offset) / VM.sR delta_limit = abs(actual_delta) + abs( 3.0 * self.mpc_solution[0].rate[0]) self.cur_state[0].delta = clip(self.cur_state[0].delta, -delta_limit, delta_limit) # account for actuation delay self.cur_state = calc_states_after_delay( self.cur_state, v_ego, angle_steers - self.angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.l_poly, self.r_poly, self.p_poly, self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width) # Check for infeasable MPC solution mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) if not mpc_nans: if (self.rate_des_prev < 0) != (self.mpc_solution[0].rate[0] < 0): delta_adjust = self.cur_state[0].delta - actual_delta self.cur_state[0].delta = actual_delta self.mpc_solution[0].delta[0] -= delta_adjust self.mpc_angles[0] = float( math.degrees(self.cur_state[0].delta * VM.sR) + angle_offset_average) self.mpc_times[0] = sm.logMonoTime['model'] * 1e-9 for i in range(1, 6): self.mpc_times[i] = self.mpc_times[i - 1] + 0.05 self.mpc_rates[i - 1] = float( math.degrees(self.mpc_solution[0].rate[i - 1] * VM.sR)) self.mpc_angles[i] = (0.05 * self.mpc_rates[i - 1] + self.mpc_angles[i - 1]) self.cur_state[0].delta = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) self.angle_steers_des_mpc = float( math.degrees(self.mpc_solution[0].delta[1] * VM.sR) + angle_offset_average) else: self.libmpc.init(MPC_COST_LAT.PATH * 0.1, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) rate_desired = 0.0 if cur_time > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = cur_time cloudlog.warning("Lateral mpc - nan: True") self.rate_des_prev = rate_desired if self.mpc_solution[ 0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message() plan_send.init('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=[ 'carState', 'controlsState', 'liveParameters', 'model' ]) plan_send.pathPlan.laneWidth = float(self.MP.lane_width) plan_send.pathPlan.pPoly = [float(x) for x in self.MP.p_poly] plan_send.pathPlan.pProb = float(self.MP.p_prob) plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly] plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly] plan_send.pathPlan.cProb = float(self.MP.c_prob) plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly] plan_send.pathPlan.lProb = float(self.MP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.r_poly] plan_send.pathPlan.rProb = float(self.MP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleBias = float(self.angle_offset - angle_offset_average) plan_send.pathPlan.angleOffset = float(angle_offset_average) plan_send.pathPlan.mpcAngles = [float(x) for x in self.mpc_angles] plan_send.pathPlan.mpcTimes = [float(x) for x in self.mpc_times] plan_send.pathPlan.mpcRates = [float(x) for x in self.mpc_rates] plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool( sm['liveParameters'].posenetValid) self.plan.send(plan_send.to_bytes()) dat = messaging.new_message() dat.init('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost self.livempc.send(dat.to_bytes())