def plannerd_thread(): gc.disable() # start the loop set_realtime_priority(2) params = Params() # Get FCW toggle from settings fcw_enabled = params.get("IsFcwEnabled") == "1" cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP, fcw_enabled) PP = PathPlanner(CP) VM = VehicleModel(CP) sm = messaging.SubMaster( ['carState', 'controlsState', 'radarState', 'model', 'liveParameters']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() if sm.updated['model']: PP.update(sm, CP, VM) if sm.updated['radarState']: PL.update(sm, CP, VM, PP)
def plannerd_thread(sm=None, pm=None): config_realtime_process(2, Priority.CTRL_LOW) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'modelLongButton'], poll=['radarState', 'model']) if pm is None: pm = messaging.PubMaster(['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() if sm.updated['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP)
def plannerd_thread(gctx): context = zmq.Context() poller = zmq.Poller() carstate = messaging.sub_sock(context, service_list['carState'].port, poller) live20 = messaging.sub_sock(context, service_list['live20'].port) model = messaging.sub_sock(context, service_list['model'].port) plan = messaging.pub_sock(context, service_list['plan'].port) # wait for stats about the car to come in from controls cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams") CS = None PP = PathPlanner(model) AC = AdaptiveCruise(live20) # start the loop set_realtime_priority(2) # this runs whenever we get a packet that can change the plan while True: polld = poller.poll(timeout=1000) for sock, mode in polld: if mode != zmq.POLLIN or sock != carstate: continue cur_time = sec_since_boot() CS = messaging.recv_sock(carstate).carState PP.update(cur_time, CS.vEgo) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? AC.update(cur_time, CS.vEgo, CS.steeringAngle, CS.vEgo, CP) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') # lateral plan plan_send.plan.lateralValid = not PP.dead if plan_send.plan.lateralValid: plan_send.plan.dPoly = map(float, PP.d_poly) # longitudal plan plan_send.plan.longitudinalValid = not AC.dead if plan_send.plan.longitudinalValid: plan_send.plan.vTarget = float(AC.v_target_lead) plan_send.plan.aTargetMin = float(AC.a_target[0]) plan_send.plan.aTargetMax = float(AC.a_target[1]) plan_send.plan.jerkFactor = float(AC.jerk_factor) plan_send.plan.hasLead = AC.has_lead plan.send(plan_send.to_bytes())
def plannerd_thread(): context = zmq.Context() params = Params() # Get FCW toggle from settings fcw_enabled = params.get("IsFcwEnabled") == "1" cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP, fcw_enabled) PP = PathPlanner(CP) VM = VehicleModel(CP) poller = zmq.Poller() car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller) live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller) model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller) live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller) car_state = messaging.new_message() car_state.init('carState') live100 = messaging.new_message() live100.init('live100') model = messaging.new_message() model.init('model') live20 = messaging.new_message() live20.init('live20') live_map_data = messaging.new_message() live_map_data.init('liveMapData') 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 while True: for socket, event in poller.poll(): if socket is live100_sock: live100 = messaging.recv_one(socket) elif socket is car_state_sock: car_state = messaging.recv_one(socket) elif socket is live_parameters_sock: live_parameters = messaging.recv_one(socket) elif socket is model_sock: model = messaging.recv_one(socket) PP.update(CP, VM, car_state, model, live100, live_parameters) elif socket is live_map_data_sock: live_map_data = messaging.recv_one(socket) elif socket is live20_sock: live20 = messaging.recv_one(socket) PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
def plannerd_thread(sm=None, pm=None, arne_sm=None): gc.disable() # start the loop set_realtime_priority(52) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster([ 'carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'liveMapData' ]) if arne_sm is None: arne_sm = messaging_arne.SubMaster( ['arne182Status', 'latControl', 'modelLongButton']) if pm is None: pm = messaging.PubMaster( ['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 while True: sm.update() arne_sm.update(0) if sm.updated['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP, arne_sm)
def plannerd_thread(sm=None, pm=None): gc.disable() # start the loop set_realtime_priority(52) cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP) PP = PathPlanner(CP) VM = VehicleModel(CP) if sm is None: sm = messaging.SubMaster([ 'carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf' ]) if pm is None: pm = messaging.PubMaster( ['plan', 'liveLongitudinalMpc', 'pathPlan', 'liveMpc']) sm['liveParameters'].valid = True sm['liveParameters'].sensorValid = True sm['liveParameters'].steerRatio = CP.steerRatio sm['liveParameters'].stiffnessFactor = 1.0 sm['dragonConf'].dpSlowOnCurve = False sm['dragonConf'].dpAccelProfile = 0 while True: sm.update() if sm.updated['model']: PP.update(sm, pm, CP, VM) if sm.updated['radarState']: PL.update(sm, pm, CP, VM, PP)
class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) self.model = messaging.sub_sock(context, service_list['model'].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.last_md_ts = 0 self.last_l20_ts = 0 self.last_model = 0. self.last_l20 = 0. self.model_dead = True self.radar_dead = True self.radar_errors = [] self.PP = PathPlanner() 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.acc_start_time = sec_since_boot() self.v_acc = 0.0 self.v_acc_sol = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.lead_1 = None self.lead_2 = None self.longitudinalPlanSource = 'cruise' self.fcw = False self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled 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) if _DEBUG: print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise 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]) # this runs whenever we get a packet that can change the plan def update(self, CS, LoC, v_cruise_kph, user_distracted): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = None l20 = None for socket, event in self.poller.poll(0): if socket is self.model: md = messaging.recv_one(socket) elif socket is self.live20: l20 = messaging.recv_one(socket) if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md) if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 # Calculate speed for normal cruise control if enabled: accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) if user_distracted: # if user is not responsive to awareness alerts, then start a smooth deceleration accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) accel_limits[0] = min(accel_limits[0], accel_limits[1]) 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 = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.vEgo 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.v_acc_sol = reset_speed self.a_acc_sol = 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, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.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.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.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 CS.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
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.radarName == "mock" VM = VehicleModel(CP) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.radarName) exec('from selfdrive.radar.'+CP.radarName+'.interface import 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) PP = PathPlanner() RI = RadarInterface() 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_array = np.zeros([2, 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) 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_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1) v_ego_array = v_ego_array[:, 1:] 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 *** PP.update(v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), False) else: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.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(PP.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=0)[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_array[1], v_ego_array[0]) 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: lead_clusters[0].toLive20(dat.live20.leadOne) if lead2_len > 0: lead2_clusters[0].toLive20(dat.live20.leadTwo) 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" % \ (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) dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) dat.liveTracks[cnt].stationary = tracks[ids].stationary dat.liveTracks[cnt].oncoming = tracks[ids].oncoming liveTracks.send(dat.to_bytes()) rk.monitor_time()
def radard_thread(gctx=None): #print "===>>> File: controls/radard.py; FUnction: radard_thread" set_realtime_priority(1) # 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)) cloudlog.info("radard got CarParams") # import the radar from the fingerprint cloudlog.info("radard is importing %s", CP.radarName) exec('from selfdrive.radar.'+CP.radarName+'.interface import RadarInterface') context = zmq.Context() # *** subscribe to features and model from visiond model = messaging.sub_sock(context, service_list['model'].port) live100 = messaging.sub_sock(context, service_list['live100'].port) PP = PathPlanner() RI = RadarInterface() 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 rdr_delay = 0.10 # radar data delay in s v_len = 20 # how many speed data points to remember for t alignment with rdr data enabled = 0 steer_angle = 0. tracks = defaultdict(dict) # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) # v_ego v_ego = None v_ego_array = np.zeros([2, 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.aRel, None, False, None] # receive the live100s l100 = messaging.recv_sock(live100) if l100 is not None: enabled = l100.live100.enabled v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1) v_ego_array = v_ego_array[:, 1:] last_l100_ts = l100.logMonoTime if v_ego is None: continue md = messaging.recv_sock(model) #print "============ RADAR Thread" #print md if md is not None: last_md_ts = md.logMonoTime # *** get path prediction from the model *** PP.update(sec_since_boot(), v_ego, md) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), np.nan, last_md_ts, np.nan, sec_since_boot()) else: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.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 enabled: # use path from model path_poly path_y = np.polyval(PP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, CP, angle_offset=0)[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 the vision point for now if ids == VISION_POINT and not VISION_ONLY: continue elif ids != VISION_POINT and VISION_ONLY: 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 - rdr_delay, v_ego_array[1], v_ego_array[0]) d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) # create the track if it doesn't exist or it's a new track if ids not in tracks or rpt[5] == 1: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2]) tracks[ids].mix_vision(dist_to_vision, rel_speed_diff) # publish tracks (debugging) dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) dat.liveTracks[cnt].stationary = tracks[ids].stationary dat.liveTracks[cnt].oncoming = tracks[ids].oncoming liveTracks.send(dat.to_bytes()) 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 = [] # *** 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.l100MonoTime = last_l100_ts if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) if lead2_len > 0: lead2_clusters[0].toLive20(dat.live20.leadTwo) else: dat.live20.leadTwo.status = False else: dat.live20.leadOne.status = False dat.live20.cumLagMs = -rk.remaining*1000. live20.send(dat.to_bytes()) rk.monitor_time()
class Planner(object): def __init__(self, CP, fcw_enabled): context = zmq.Context() self.CP = CP self.poller = zmq.Poller() self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller) self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller) self.live_map_data = messaging.sub_sock( context, service_list['liveMapData'].port, conflate=True, poller=self.poller) if os.environ.get('GPS_PLANNER_ACTIVE', False): self.gps_planner_plan = messaging.sub_sock( context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR) else: self.gps_planner_plan = None self.plan = messaging.pub_sock(context, service_list['plan'].port) self.live_longitudinal_mpc = messaging.pub_sock( context, service_list['liveLongitudinalMpc'].port) self.last_md_ts = 0 self.last_l20_ts = 0 self.last_model = 0. self.last_l20 = 0. self.model_dead = True self.radar_dead = True self.radar_errors = [] self.PP = PathPlanner() 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.acc_start_time = sec_since_boot() self.v_acc = 0.0 self.v_acc_sol = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.a_acc_sol = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.lead_1 = None self.lead_2 = None self.longitudinalPlanSource = 'cruise' self.fcw = False self.fcw_checker = FCWChecker() self.fcw_enabled = fcw_enabled self.last_gps_planner_plan = None self.gps_planner_active = False self.last_live_map_data = None self.perception_state = log.Live20Data.new_message() self.params = Params() self.v_curvature = NO_CURVATURE_SPEED self.v_speedlimit = NO_CURVATURE_SPEED self.decel_for_turn = False self.map_valid = False 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) """ print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise """ 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 ]) # this runs whenever we get a packet that can change the plan def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): cur_time = sec_since_boot() v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS md = None l20 = None gps_planner_plan = None for socket, event in self.poller.poll(0): if socket is self.model: md = messaging.recv_one(socket) elif socket is self.live20: l20 = messaging.recv_one(socket) elif socket is self.gps_planner_plan: gps_planner_plan = messaging.recv_one(socket) elif socket is self.live_map_data: self.last_live_map_data = messaging.recv_one( socket).liveMapData if gps_planner_plan is not None: self.last_gps_planner_plan = gps_planner_plan if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False self.PP.update(CS.vEgo, md, LaC) if self.last_gps_planner_plan is not None: plan = self.last_gps_planner_plan.gpsPlannerPlan self.gps_planner_active = plan.valid if plan.valid: self.PP.d_poly = plan.poly self.PP.p_poly = plan.poly self.PP.c_poly = plan.poly self.PP.l_prob = 0.0 self.PP.r_prob = 0.0 self.PP.c_prob = 1.0 if l20 is not None: self.perception_state = copy(l20.live20) self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) self.v_acc_start = self.v_acc_sol self.a_acc_start = self.a_acc_sol self.acc_start_time = cur_time self.lead_1 = l20.live20.leadOne self.lead_2 = l20.live20.leadTwo enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0 if self.last_live_map_data: self.v_speedlimit = NO_CURVATURE_SPEED self.v_curvature = NO_CURVATURE_SPEED self.map_valid = self.last_live_map_data.mapValid # Speed limit if self.last_live_map_data.speedLimitValid: speed_limit = self.last_live_map_data.speedLimit set_speed_limit_active = self.params.get( "LimitSetSpeed") == "1" and self.params.get( "SpeedLimitOffset") is not None if set_speed_limit_active: offset = float(self.params.get("SpeedLimitOffset")) self.v_speedlimit = speed_limit + offset # Curvature if self.last_live_map_data.curvatureValid: curvature = abs(self.last_live_map_data.curvature) v_curvature = math.sqrt(A_Y_MAX / max(1e-4, curvature)) self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) # leave 1m/s margin on vEgo to asses if turn is limiting our speed. self.decel_for_turn = bool(self.v_curvature < min( [v_cruise_setpoint, self.v_speedlimit, CS.vEgo + 1.])) v_cruise_setpoint = min( [v_cruise_setpoint, self.v_curvature, self.v_speedlimit]) # Calculate speed for normal cruise control if enabled: accel_limits = map( float, calc_cruise_accel_limits(CS.vEgo, following)) # TODO: make a separate lookup for jerk tuning jerk_limits = [ min(-0.1, accel_limits[0]), max(0.1, accel_limits[1]) ] accel_limits = limit_accel_in_turns(CS.vEgo, CS.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 self.decel_for_turn: time_to_turn = max( 1.0, self.last_live_map_data.distToTurn / max(self.v_cruise, 1.)) required_decel = min( 0, (self.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 = LoC.long_control_state == LongCtrlState.starting a_ego = min(CS.aEgo, 0.0) reset_speed = MIN_CAN_SPEED if starting else CS.vEgo 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.v_acc_sol = reset_speed self.a_acc_sol = 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, self.lead_1, v_cruise_setpoint) self.mpc2.update(CS, self.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.leftBlinker or CS.rightBlinker self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.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 CS.brakePressed if self.fcw: cloudlog.info("FCW triggered %s", self.fcw_checker.counters) if cur_time - self.last_model > 0.5: self.model_dead = True if cur_time - self.last_l20 > 0.5: self.radar_dead = True # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if LaC.mpc_solution[ 0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge events.append( create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) # Interpolation of trajectory dt = min( cur_time - self.acc_start_time, _DT_MPC + _DT ) + _DT # no greater than dt mpc + dt, to prevent too high extraps self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start) self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0 plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vCruise = self.v_cruise plan_send.plan.aCruise = self.a_cruise plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.hasLeftLane = bool(self.PP.l_prob > 0.5) plan_send.plan.hasRightLane = bool(self.PP.r_prob > 0.5) plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.gpsPlannerActive = self.gps_planner_active plan_send.plan.vCurvature = self.v_curvature plan_send.plan.decelForTurn = self.decel_for_turn plan_send.plan.mapValid = self.map_valid # Send out fcw fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) plan_send.plan.fcw = fcw self.plan.send(plan_send.to_bytes()) return plan_send
def controlsd_thread(gctx, rate=100): #rate in Hz # *** log *** context = zmq.Context() live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) thermal = messaging.sub_sock(context, service_list['thermal'].port) live20 = messaging.sub_sock(context, service_list['live20'].port) model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) logcan = messaging.sub_sock(context, service_list['can'].port) # connects to can and sendcan CI = CarInterface() VP = CI.getVehicleParams() PP = PathPlanner(model) AC = AdaptiveCruise(live20) AM = AlertManager() LoC = LongControl() LaC = LatControl() # controls enabled state enabled = False last_enable_request = 0 # learned angle offset angle_offset = 0 # rear view camera state rear_view_toggle = False v_cruise_kph = 255 # 0.0 - 1.0 awareness_status = 0.0 soft_disable_timer = None # Is cpu temp too high to enable? overtemp = False free_space = 1.0 # start the loop set_realtime_priority(2) rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) while 1: prof = Profiler() cur_time = sec_since_boot() # read CAN # CS = CI.update() CS = car.CarState.new_message() CS.vEgo = 13 for a in messaging.drain_sock(logcan): CS.steeringAngle = a.carState.steeringAngle # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = CS # copy? carstate.send(cs_send.to_bytes()) prof.checkpoint("CarInterface") # did it request to enable? enable_request, enable_condition = False, False if enabled: # gives the user 6 minutes # awareness_status -= 1.0/(100*60*6) if awareness_status <= 0.: # AM.add("driverDistracted", enabled) awareness_status = 1.0 # reset awareness status on steering if CS.steeringPressed: awareness_status = 1.0 # handle button presses for b in CS.buttonEvents: print b # reset awareness on any user action awareness_status = 1.0 # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed: rear_view_toggle = True else: rear_view_toggle = False if b.type == "altButton1" and b.pressed: rear_view_toggle = not rear_view_toggle if not VP.brake_only and enabled and not b.pressed: if b.type == "accelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA elif b.type == "decelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) if not enabled and b.type in ["accelCruise", "decelCruise" ] and not b.pressed: enable_request = True # do disable on button down if b.type == "cancel" and b.pressed: AM.add("disable", enabled) # Hack-hack if not enabled: enable_request = True prof.checkpoint("Buttons") # *** health checking logic *** hh = messaging.recv_sock(health) if hh is not None: # if the board isn't allowing controls but somehow we are enabled! if not hh.health.controlsAllowed and enabled: AM.add("controlsMismatch", enabled) # *** thermal checking logic *** # thermal data, checked every second td = messaging.recv_sock(thermal) if False and td is not None: # Check temperature. overtemp = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) # under 15% of space free free_space = td.thermal.freeSpace prof.checkpoint("Health") # *** getting model logic *** PP.update(cur_time, CS.vEgo) if rk.frame % 5 == 2: # *** run this at 20hz again *** angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steeringPressed) # disable if the pedals are pressed while engaged, this is a user disable if enabled: if CS.gasPressed or CS.brakePressed: AM.add("disable", enabled) if enable_request: # check for pressed pedals if CS.gasPressed or CS.brakePressed: AM.add("pedalPressed", enabled) enable_request = False else: print "enabled pressed at", cur_time last_enable_request = cur_time # don't engage with less than 15% free if free_space < 0.15: AM.add("outOfSpace", enabled) enable_request = False if VP.brake_only: enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled else: enable_condition = enable_request if enable_request or enable_condition or enabled: # add all alerts from car for alert in CS.errors: AM.add(alert, enabled) if False and AC.dead: AM.add("radarCommIssue", enabled) if PP.dead: AM.add("modelCommIssue", enabled) if overtemp: AM.add("overheat", enabled) prof.checkpoint("Model") if enable_condition and not enabled and not AM.alertPresent(): print "*** enabling controls" # beep for enabling AM.add("enable", enabled) # enable both lateral and longitudinal controls enabled = True # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int( round( max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN))) # 6 minutes driver you're on awareness_status = 1.0 # reset the PID loops LaC.reset() # start long control at actual speed LoC.reset(v_pid=CS.vEgo) if VP.brake_only and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # *** put the adaptive in adaptive cruise control *** AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, awareness_status, VP) prof.checkpoint("AdaptiveCruise") # *** gas/brake PID loop *** final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph, AC.v_target_lead, AC.a_target, AC.jerk_factor, VP) # *** steering PID loop *** final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, PP.d_poly, angle_offset, VP) prof.checkpoint("PID") # ***** handle alerts **** # send a "steering required alert" if saturation count has reached the limit if False and sat_flag: AM.add("steerSaturated", enabled) if enabled and AM.alertShouldDisable(): print "DISABLING IMMEDIATELY ON ALERT" enabled = False if enabled and AM.alertShouldSoftDisable(): if soft_disable_timer is None: soft_disable_timer = 3 * rate elif soft_disable_timer == 0: print "SOFT DISABLING ON ALERT" enabled = False else: soft_disable_timer -= 1 else: soft_disable_timer = None # *** push the alerts to current *** alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts( cur_time) # ***** control the car ***** CC = car.CarControl.new_message() CC.enabled = enabled CC.gas = float(final_gas) CC.brake = float(final_brake) CC.steeringTorque = float(final_steer) CC.cruiseControl.override = True CC.cruiseControl.cancel = bool( (not VP.brake_only) or (not enabled and CS.cruiseState.enabled )) # always cancel if we have an interceptor CC.cruiseControl.speedOverride = float((LoC.v_pid - .3) if ( VP.brake_only and final_brake == 0.) else 0.0) CC.cruiseControl.accelOverride = float(AC.a_pcm) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = enabled CC.hudControl.lanesVisible = enabled CC.hudControl.leadVisible = bool(AC.has_lead) CC.hudControl.visualAlert = visual_alert CC.hudControl.audibleAlert = audible_alert # this alert will apply next controls cycle #if not CI.apply(CC): # AM.add("controlsFailed", enabled) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = CC # copy? #carcontrol.send(cc_send.to_bytes()) sendcan.send(cc_send.to_bytes()) prof.checkpoint("CarControl") # ***** publish state to logger ***** # publish controls state at 100Hz dat = messaging.new_message() dat.init('live100') # show rear view camera on phone if in reverse gear or when button is pressed dat.live100.rearViewCam = ('reverseGear' in CS.errors) or rear_view_toggle dat.live100.alertText1 = alert_text_1 dat.live100.alertText2 = alert_text_2 dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0 # what packets were used to process dat.live100.canMonoTimes = list(CS.canMonoTimes) dat.live100.mdMonoTime = PP.logMonoTime dat.live100.l20MonoTime = AC.logMonoTime # if controls is enabled dat.live100.enabled = enabled # car state dat.live100.vEgo = CS.vEgo dat.live100.angleSteers = CS.steeringAngle dat.live100.steerOverride = CS.steeringPressed # longitudinal control state dat.live100.vPid = float(LoC.v_pid) dat.live100.vCruise = float(v_cruise_kph) dat.live100.upAccelCmd = float(LoC.Up_accel_cmd) dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd) # lateral control state dat.live100.yActual = float(LaC.y_actual) dat.live100.yDes = float(LaC.y_des) dat.live100.upSteer = float(LaC.Up_steer) dat.live100.uiSteer = float(LaC.Ui_steer) # processed radar state, should add a_pcm? dat.live100.vTargetLead = float(AC.v_target_lead) dat.live100.aTargetMin = float(AC.a_target[0]) dat.live100.aTargetMax = float(AC.a_target[1]) dat.live100.jerkFactor = float(AC.jerk_factor) # lag dat.live100.cumLagMs = -rk.remaining * 1000. live100.send(dat.to_bytes()) prof.checkpoint("Live100") # *** run loop at fixed rate *** if rk.keep_time(): prof.display()
class Planner(object): def __init__(self, CP): context = zmq.Context() self.CP = CP self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.model = messaging.sub_sock(context, service_list['model'].port) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.last_md_ts = 0 self.last_l20_ts = 0 if os.getenv("OPT") is not None: self.PP = OptPathPlanner(self.model) else: self.PP = PathPlanner() self.AC = AdaptiveCruise() self.FCW = ForwardCollisionWarning(_DT) # this runs whenever we get a packet that can change the plan def update(self, CS, LoC): #print "===>>> File: controls/lib/planner.py; FUnction: update" cur_time = sec_since_boot() md = messaging.recv_sock(self.model) #print "============ Planner" #print md if md is not None: self.last_md_ts = md.logMonoTime # print "========= frameId" # print md.model.frameId # else: # print "======== None" l20 = messaging.recv_sock(self.live20) if l20 is not None: self.last_l20_ts = l20.logMonoTime self.PP.update(cur_time, CS.vEgo, md) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? self.AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.PP.dead if plan_send.plan.lateralValid: plan_send.plan.dPoly = map(float, self.PP.d_poly) #else: #print "====================No Vision Data========" # longitudal plan plan_send.plan.longitudinalValid = not self.AC.dead if plan_send.plan.longitudinalValid: plan_send.plan.vTarget = float(self.AC.v_target_lead) plan_send.plan.aTargetMin = float(self.AC.a_target[0]) plan_send.plan.aTargetMax = float(self.AC.a_target[1]) plan_send.plan.jerkFactor = float(self.AC.jerk_factor) plan_send.plan.hasLead = self.AC.has_lead # compute risk of collision events: fcw self.FCW.process(CS, self.AC) plan_send.plan.fcw = bool(self.FCW.active) self.plan.send(plan_send.to_bytes()) return plan_send
def radard_thread(gctx=None): set_realtime_priority(1) context = zmq.Context() # *** subscribe to features and model from visiond model = messaging.sub_sock(context, service_list['model'].port) logcan = messaging.sub_sock(context, service_list['can'].port) live100 = messaging.sub_sock(context, service_list['live100'].port) PP = PathPlanner(model) # *** publish live20 and liveTracks live20 = messaging.pub_sock(context, service_list['live20'].port) liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port) # subscribe to stats about the car # TODO: move this to new style packet VP = VehicleParams(False) # same for ILX and civic ar_pts = {} 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 rdr_delay = 0.10 # radar data delay in s v_len = 20 # how many speed data points to remember for t alignment with rdr data enabled = 0 steer_angle = 0. tracks = defaultdict(dict) # Nidec cp = _create_radard_can_parser() # Kalman filter stuff: ekfv = EKFV1D() speedSensorV = SimpleSensor(XV, 1, 2) # v_ego v_ego = None v_ego_array = np.zeros([2, v_len]) v_ego_t_aligned = 0. rk = Ratekeeper(rate, print_delay_threshold=np.inf) while 1: canMonoTimes = [] can_pub_radar = [] for a in messaging.drain_sock(logcan, wait_for_one=True): canMonoTimes.append(a.logMonoTime) can_pub_radar.extend(can_capnp_to_can_list_old(a, [1, 3])) # only run on the 0x445 packets, used for timing if not any(x[0] == 0x445 for x in can_pub_radar): continue cp.update_can(can_pub_radar) if not cp.can_valid: # TODO: handle this pass ar_pts = nidec_decode(cp, ar_pts) # receive the live100s l100 = messaging.recv_sock(live100) if l100 is not None: enabled = l100.live100.enabled v_ego = l100.live100.vEgo steer_angle = l100.live100.angleSteers v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame) / rate]], 1) v_ego_array = v_ego_array[:, 1:] if v_ego is None: continue # *** get path prediction from the model *** PP.update(sec_since_boot(), v_ego) # run kalman filter only if prob is high enough if PP.lead_prob > 0.7: ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var)) ekfv.predict(tsv) ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), float(ekfv.state[SPEEDV]), np.nan, PP.logMonoTime, np.nan, sec_since_boot()) else: ekfv.state[XV] = PP.lead_dist ekfv.covar = (np.diag([PP.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 enabled: # use path from model path_poly path_y = np.polyval(PP.d_poly, path_x) else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VP, angle_offset=0)[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 the vision point for now if ids == VISION_POINT: 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 - rdr_delay, v_ego_array[1], v_ego_array[0]) d_path = np.sqrt( np.amin((path_x - rpt[0])**2 + (path_y - rpt[1])**2)) # create the track if ids not in tracks or rpt[5] == 1: tracks[ids] = Track() tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned) # allow the vision model to remove the stationary flag if distance and rel speed roughly match if VISION_POINT in ar_pts: dist_to_vision = np.sqrt( (0.5 * (ar_pts[VISION_POINT][0] - rpt[0]))**2 + (2 * (ar_pts[VISION_POINT][1] - rpt[1]))**2) rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2]) tracks[ids].mix_vision(dist_to_vision, rel_speed_diff) # publish tracks (debugging) dat = messaging.new_message() dat.init('liveTracks', len(tracks)) for cnt, ids in enumerate(tracks.keys()): dat.liveTracks[cnt].trackId = ids dat.liveTracks[cnt].dRel = float(tracks[ids].dRel) dat.liveTracks[cnt].yRel = float(tracks[ids].yRel) dat.liveTracks[cnt].vRel = float(tracks[ids].vRel) dat.liveTracks[cnt].aRel = float(tracks[ids].aRel) dat.liveTracks[cnt].stationary = tracks[ids].stationary dat.liveTracks[cnt].oncoming = tracks[ids].oncoming liveTracks.send(dat.to_bytes()) 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 = [] # *** extract the lead car *** lead_clusters = [ c for c in clusters if c.is_potential_lead(v_ego, enabled) ] 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 = PP.logMonoTime dat.live20.canMonoTimes = canMonoTimes if lead_len > 0: lead_clusters[0].toLive20(dat.live20.leadOne) if lead2_len > 0: lead2_clusters[0].toLive20(dat.live20.leadTwo) else: dat.live20.leadTwo.status = False else: dat.live20.leadOne.status = False dat.live20.cumLagMs = -rk.remaining * 1000. live20.send(dat.to_bytes()) rk.monitor_time()
def plannerd_thread(): gc.disable() # start the loop set_realtime_priority(2) context = zmq.Context() params = Params() # Get FCW toggle from settings fcw_enabled = params.get("IsFcwEnabled") == "1" cloudlog.info("plannerd is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) PL = Planner(CP, fcw_enabled) PP = PathPlanner(CP) VM = VehicleModel(CP) poller = zmq.Poller() car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller) controls_state_sock = messaging.sub_sock(context, service_list['controlsState'].port, conflate=True, poller=poller) radar_state_sock = messaging.sub_sock(context, service_list['radarState'].port, conflate=True, poller=poller) model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller) # live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller) car_state = messaging.new_message() car_state.init('carState') controls_state = messaging.new_message() controls_state.init('controlsState') model = messaging.new_message() model.init('model') radar_state = messaging.new_message() radar_state.init('radarState') live_map_data = messaging.new_message() live_map_data.init('liveMapData') live_parameters = messaging.new_message() live_parameters.init('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True live_parameters.liveParameters.steerRatio = CP.steerRatio live_parameters.liveParameters.stiffnessFactor = 1.0 rcv_times = defaultdict(int) while True: for socket, event in poller.poll(): msg = messaging.recv_one(socket) rcv_times[msg.which()] = sec_since_boot() if socket is controls_state_sock: controls_state = msg elif socket is car_state_sock: car_state = msg elif socket is live_parameters_sock: live_parameters = msg elif socket is model_sock: model = msg PP.update(rcv_times, CP, VM, car_state, model, controls_state, live_parameters) elif socket is radar_state_sock: radar_state = msg PL.update(rcv_times, car_state, CP, VM, PP, radar_state, controls_state, model, live_map_data)
class Planner(object): def __init__(self, CP): context = zmq.Context() self.CP = CP self.live20 = messaging.sub_sock(context, service_list['live20'].port) self.model = messaging.sub_sock(context, service_list['model'].port) self.plan = messaging.pub_sock(context, service_list['plan'].port) self.last_md_ts = 0 self.last_l20_ts = 0 self.last_model = 0. self.last_l20 = 0. self.model_dead = True self.radar_dead = True self.radar_errors = [] self.PP = PathPlanner() self.AC = AdaptiveCruise() self.FCW = ForwardCollisionWarning(_DT) # this runs whenever we get a packet that can change the plan def update(self, CS, LoC): cur_time = sec_since_boot() md = messaging.recv_sock(self.model) if md is not None: self.last_md_ts = md.logMonoTime self.last_model = cur_time self.model_dead = False if cur_time - self.last_model > 0.5: self.model_dead = True l20 = messaging.recv_sock(self.live20) if l20 is not None: self.last_l20_ts = l20.logMonoTime self.last_l20 = cur_time self.radar_dead = False self.radar_errors = list(l20.live20.radarErrors) if cur_time - self.last_l20 > 0.5: self.radar_dead = True self.PP.update(CS.vEgo, md) # LoC.v_pid -> CS.vEgo # TODO: is this change okay? self.AC.update(CS.vEgo, CS.steeringAngle, LoC.v_pid, self.CP, l20) # **** send the plan **** plan_send = messaging.new_message() plan_send.init('plan') events = [] if self.model_dead: events.append( create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.radar_dead or 'commIssue' in self.radar_errors: events.append( create_event('radarCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if 'fault' in self.radar_errors: events.append( create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) plan_send.plan.events = events plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.l20MonoTime = self.last_l20_ts # lateral plan plan_send.plan.lateralValid = not self.model_dead plan_send.plan.dPoly = map(float, self.PP.d_poly) plan_send.plan.laneWidth = float(self.PP.lane_width) # longitudal plan plan_send.plan.longitudinalValid = not self.radar_dead plan_send.plan.vTarget = float(self.AC.v_target_lead) plan_send.plan.aTargetMin = float(self.AC.a_target[0]) plan_send.plan.aTargetMax = float(self.AC.a_target[1]) plan_send.plan.jerkFactor = float(self.AC.jerk_factor) plan_send.plan.hasLead = self.AC.has_lead # compute risk of collision events: fcw self.FCW.process(CS, self.AC) plan_send.plan.fcw = bool(self.FCW.active) self.plan.send(plan_send.to_bytes()) return plan_send