def update(self, sm, rr, enable_lead): self.current_time = 1e-9*max(sm.logMonoTime.values()) if sm.updated['carState']: self.v_ego = sm['carState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['modelV2']: self.ready = True ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): if ids not in ar_pts: self.tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement v_lead = rpt[2] + self.v_ego_hist[0] # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track(v_lead, self.kalman_params) self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) idens = list(sorted(self.tracks.keys())) track_pts = list([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 range(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: # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 cluster_idxs = [0] clusters = [Cluster()] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] # if a new point, reset accel to the rest of the cluster for idx in range(len(track_pts)): if self.tracks[idens[idx]].cnt <= 1: aLeadK = clusters[cluster_idxs[idx]].aLeadK aLeadTau = clusters[cluster_idxs[idx]].aLeadTau self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) # *** publish radarState *** dat = messaging.new_message('radarState') dat.valid = sm.all_alive_and_valid() and len(rr.errors) == 0 radarState = dat.radarState radarState.mdMonoTime = sm.logMonoTime['modelV2'] radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: if len(sm['modelV2'].leads) > 1: radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True) radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False) return dat
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()
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 update(self, frame, sm, rr, has_radar,rrext): self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) if sm.updated['model']: self.ready = True if sm.updated['pathPlan']: self.lane_width = sm['pathPlan'].laneWidth self.dPoly = sm['pathPlan'].dPoly path_y = np.polyval(self.dPoly, self.path_x) ar_pts = {} for pt in rr.points: extpt = get_rrext_by_trackId(rrext,pt.trackId) ar_pts[pt.trackId] = [pt.dRel + RDR_TO_LDR, pt.yRel, pt.vRel, pt.measured, pt.aRel, pt.yvRel, extpt.objectClass, extpt.length, pt.trackId+2, extpt.movingState] # *** remove missing points from meta data *** for ids in list(self.tracks.keys()): if ids not in ar_pts: self.tracks.pop(ids, None) # *** compute the tracks *** for ids in ar_pts: rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement self.v_ego_t_aligned = self.v_ego_hist[0] # distance relative to path d_path = np.sqrt(np.amin((self.path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2)) # add sign d_path *= np.sign(rpt[1] - np.interp(rpt[0], self.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], rpt[3], rpt[4],rpt[5],rpt[6],rpt[7],rpt[8],rpt[9], d_path, self.v_ego_t_aligned,self.use_tesla_radar) idens = list(sorted(self.tracks.keys())) track_pts = list([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 range(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster(self.use_tesla_radar) clusters[cluster_i].add(self.tracks[idens[idx]]) elif len(track_pts) == 1: # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 cluster_idxs = [0] clusters = [Cluster(self.use_tesla_radar)] clusters[0].add(self.tracks[idens[0]]) else: clusters = [] # if a new point, reset accel to the rest of the cluster for idx in range(len(track_pts)): if self.tracks[idens[idx]].cnt <= 1: aLeadK = clusters[cluster_idxs[idx]].aLeadK aLeadTau = clusters[cluster_idxs[idx]].aLeadTau self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) ### START REVIEW SECTION ################################################################# #BB For Tesla integration we will also track Left and Right lanes ################################################################# if (self.RI.TRACK_RIGHT_LANE or self.RI.TRACK_LEFT_LANE) and self.use_tesla_radar: datrl = tesla.ICCarsLR.new_message() datrl.v1Type = int(0) datrl.v1Dx = float(0.) datrl.v1Vrel = float(0.) datrl.v1Dy = float(0.) datrl.v1Id = int(0) datrl.v2Type = int(0) datrl.v2Dx = float(0.) datrl.v2Vrel = float(0.) datrl.v2Dy = float(0.) datrl.v2Id = int(0) datrl.v3Type = int(0) datrl.v3Dx = float(0.) datrl.v3Vrel = float(0.) datrl.v3Dy = float(0.) datrl.v3Id = int(0) datrl.v4Type = int(0) datrl.v4Dx = float(0.) datrl.v4Vrel = float(0.) datrl.v4Dy = float(0.) datrl.v4Id = int(0) lane_offset = 0. #LEFT LANE if self.RI.TRACK_LEFT_LANE and self.use_tesla_radar: ll_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(-self.lane_width) for iden in idens]) # If we have multiple points, cluster them if len(ll_track_pts) > 1: ll_cluster_idxs = cluster_points_centroid(ll_track_pts, 2.5) ll_clusters = [None] * (max(ll_cluster_idxs) + 1) for idx in range(len(ll_track_pts)): ll_cluster_i = ll_cluster_idxs[idx] if ll_clusters[ll_cluster_i] == None: ll_clusters[ll_cluster_i] = Cluster(self.use_tesla_radar) ll_clusters[ll_cluster_i].add(self.tracks[idens[idx]]) elif len(ll_track_pts) == 1: # TODO: why do we need this? ll_clusters = [Cluster(self.use_tesla_radar)] ll_clusters[0].add(self.tracks[idens[0]]) else: ll_clusters = [] if DEBUG: for i in ll_clusters: print(i) # *** extract the lead car *** ll_lead_clusters = [c for c in ll_clusters if c.is_potential_lead_dy(self.v_ego,-self.lane_width)] ll_lead_clusters.sort(key=lambda x: x.dRel) ll_lead_len = len(ll_lead_clusters) ll_lead1_truck = (len([c for c in ll_lead_clusters if c.is_truck(ll_lead_clusters)]) > 0) # *** extract the second lead from the whole set of leads *** ll_lead2_clusters = [c for c in ll_lead_clusters if c.is_potential_lead2(ll_lead_clusters)] ll_lead2_clusters.sort(key=lambda x: x.dRel) ll_lead2_len = len(ll_lead2_clusters) ll_lead2_truck = (len([c for c in ll_lead_clusters if c.is_truck(ll_lead2_clusters)]) > 0) # publish data if ll_lead_len > 0: datrl.v1Type = int(ll_lead_clusters[0].oClass) if datrl.v1Type == 1 and ll_lead1_truck: datrl.v1Type = 0 datrl.v1Dx = float(ll_lead_clusters[0].dRel) datrl.v1Vrel = float(ll_lead_clusters[0].vRel) datrl.v1Dy = float(-ll_lead_clusters[0].yRel - lane_offset) datrl.v1Id = int(ll_lead_clusters[0].track_id % 32) if ll_lead2_len > 0: datrl.v2Type = int(ll_lead2_clusters[0].oClass) if datrl.v2Type == 1 and ll_lead2_truck: datrl.v2Type = 0 datrl.v2Dx = float(ll_lead2_clusters[0].dRel) datrl.v2Vrel = float(ll_lead2_clusters[0].vRel) datrl.v2Dy = float(-ll_lead2_clusters[0].yRel - lane_offset) datrl.v2Id = int(ll_lead2_clusters[0].track_id % 32) #RIGHT LANE if self.RI.TRACK_RIGHT_LANE and self.use_tesla_radar: rl_track_pts = np.array([self.tracks[iden].get_key_for_cluster_dy(self.lane_width) for iden in idens]) # If we have multiple points, cluster them if len(rl_track_pts) > 1: rl_cluster_idxs = cluster_points_centroid(rl_track_pts, 2.5) rl_clusters = [None] * (max(rl_cluster_idxs) + 1) for idx in range(len(rl_track_pts)): rl_cluster_i = rl_cluster_idxs[idx] if rl_clusters[rl_cluster_i] == None: rl_clusters[rl_cluster_i] = Cluster(self.use_tesla_radar) rl_clusters[rl_cluster_i].add(self.tracks[idens[idx]]) elif len(rl_track_pts) == 1: # TODO: why do we need this? rl_clusters = [Cluster(self.use_tesla_radar)] rl_clusters[0].add(self.tracks[idens[0]]) else: rl_clusters = [] if DEBUG: for i in rl_clusters: print(i) # *** extract the lead car *** rl_lead_clusters = [c for c in rl_clusters if c.is_potential_lead_dy(self.v_ego,self.lane_width)] rl_lead_clusters.sort(key=lambda x: x.dRel) rl_lead_len = len(rl_lead_clusters) rl_lead1_truck = (len([c for c in rl_lead_clusters if c.is_truck(rl_lead_clusters)]) > 0) # *** extract the second lead from the whole set of leads *** rl_lead2_clusters = [c for c in rl_lead_clusters if c.is_potential_lead2(rl_lead_clusters)] rl_lead2_clusters.sort(key=lambda x: x.dRel) rl_lead2_len = len(rl_lead2_clusters) rl_lead2_truck = (len([c for c in rl_lead_clusters if c.is_truck(rl_lead2_clusters)]) > 0) # publish data if rl_lead_len > 0: datrl.v3Type = int(rl_lead_clusters[0].oClass) if datrl.v3Type == 1 and rl_lead1_truck: datrl.v3Type = 0 datrl.v3Dx = float(rl_lead_clusters[0].dRel) datrl.v3Vrel = float(rl_lead_clusters[0].vRel) datrl.v3Dy = float(-rl_lead_clusters[0].yRel+ lane_offset) datrl.v3Id = int(rl_lead_clusters[0].track_id % 32) if rl_lead2_len > 0: datrl.v4Type = int(rl_lead2_clusters[0].oClass) if datrl.v4Type == 1 and rl_lead2_truck: datrl.v4Type = 0 datrl.v4Dx = float(rl_lead2_clusters[0].dRel) datrl.v4Vrel = float(rl_lead2_clusters[0].vRel) datrl.v4Dy = float(-rl_lead2_clusters[0].yRel + lane_offset) datrl.v4Id = int(rl_lead2_clusters[0].track_id % 32) if (self.RI.TRACK_RIGHT_LANE or self.RI.TRACK_LEFT_LANE) and self.use_tesla_radar: self.icCarLR.send(datrl.to_bytes()) ### END REVIEW SECTION # *** publish radarState *** dat = messaging.new_message() dat.init('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) 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 datext = tesla.ICLeads.new_message() l1x = tesla.TeslaLeadPoint.new_message() l2x = tesla.TeslaLeadPoint.new_message() if has_radar: l1d,l1x = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True,use_tesla_radar=self.use_tesla_radar) l2d,l2x = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False, use_tesla_radar=self.use_tesla_radar) dat.radarState.leadOne = l1d dat.radarState.leadTwo = l2d datext.lead1trackId = l1x['trackId'] datext.lead1oClass = l1x['oClass'] datext.lead1length = l1x['length'] datext.lead2trackId = l2x['trackId'] datext.lead2oClass = l2x['oClass'] datext.lead2length = l2x['length'] return dat, datext
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 update(self, frame, delay, sm, rr, has_radar): self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo self.v_ego_hist_v.append(self.v_ego) self.v_ego_hist_t.append(float(frame)/rate) if sm.updated['model']: self.ready = True ar_pts = {} for pt in rr.points: ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** 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: 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) # 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], self.v_ego_t_aligned, rpt[3]) 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: clusters = [Cluster()] clusters[0].add(self.tracks[idens[0]]) else: 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 has_radar: dat.radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) dat.radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) return dat
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
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" 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) controls_state_sock = messaging.sub_sock( context, service_list['controlsState'].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_controls_state_ts = 0 # *** publish radarState and liveTracks radarState = messaging.pub_sock(context, service_list['radarState'].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=None) 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 controlsStates controls_state = None md = None for socket, event in poller.poll(0): if socket is controls_state_sock: controls_state = 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 controls_state is not None: active = controls_state.controlsState.active v_ego = controls_state.controlsState.vEgo steer_angle = controls_state.controlsState.angleSteers steer_override = controls_state.controlsState.steerOverride v_ego_hist_v.append(v_ego) v_ego_hist_t.append(float(rk.frame) / rate) last_controls_state_ts = controls_state.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 = list(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: 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(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 radarState *** dat = messaging.new_message() dat.init('radarState') dat.radarState.mdMonoTime = last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) dat.radarState.radarErrors = list(rr.errors) dat.radarState.controlsStateMonoTime = 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 dat.radarState.cumLagMs = -rk.remaining * 1000. radarState.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()