diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 930e8606fd..cc55764410 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -53,7 +53,7 @@ class Controls: self.sm = sm if self.sm is None: ignore = ['ubloxRaw', 'frontFrame'] if SIMULATION else None - self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'ubloxRaw', + self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'frame', 'frontFrame'], ignore_alive=ignore) @@ -237,7 +237,7 @@ class Controls: self.events.add(EventName.noGps) if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL): self.events.add(EventName.cameraMalfunction) - if self.sm['model'].frameDropPerc > 20: + if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) # Only allow engagement with brake pressed when stopped behind another stopped car @@ -433,12 +433,12 @@ class Controls: ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED - meta = self.sm['model'].meta + meta = self.sm['modelV2'].meta if len(meta.desirePrediction) and ldw_allowed: l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1] - l_lane_close = left_lane_visible and (self.sm['model'].leftLane.poly[3] < (1.08 - CAMERA_OFFSET)) - r_lane_close = right_lane_visible and (self.sm['model'].rightLane.poly[3] > -(1.08 + CAMERA_OFFSET)) + l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET)) CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 3861bf5743..132bba8bd3 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -132,11 +132,11 @@ class Cluster(): def get_RadarState_from_vision(self, lead_msg, v_ego): return { - "dRel": float(lead_msg.dist - RADAR_TO_CAMERA), - "yRel": float(lead_msg.relY), - "vRel": float(lead_msg.relVel), - "vLead": float(v_ego + lead_msg.relVel), - "vLeadK": float(v_ego + lead_msg.relVel), + "dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.xyva[1]), + "vRel": float(lead_msg.xyva[2]), + "vLead": float(v_ego + lead_msg.xyva[2]), + "vLeadK": float(v_ego + lead_msg.xyva[2]), "aLeadK": float(0), "aLeadTau": _LEAD_ACCEL_TAU, "fcw": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6cb5f36a0a..ca75146062 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -37,12 +37,12 @@ def laplacian_cdf(x, mu, b): def match_vision_to_cluster(v_ego, lead, clusters): # match vision point to best statistical cluster match - offset_vision_dist = lead.dist - RADAR_TO_CAMERA + offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA def prob(c): - prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) - prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) - prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0]) + prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1]) + prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -52,7 +52,7 @@ def match_vision_to_cluster(v_ego, lead, clusters): # if no 'sane' match is found return -1 # stationary radar points can be false positives dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 3) + vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3) if dist_sane and vel_sane: return cluster else: @@ -103,7 +103,7 @@ class RadarD(): if sm.updated['controlsState']: self.v_ego = sm['controlsState'].vEgo self.v_ego_hist.append(self.v_ego) - if sm.updated['model']: + if sm.updated['modelV2']: self.ready = True ar_pts = {} @@ -159,14 +159,15 @@ class RadarD(): dat = messaging.new_message('radarState') dat.valid = sm.all_alive_and_valid() radarState = dat.radarState - radarState.mdMonoTime = sm.logMonoTime['model'] + radarState.mdMonoTime = sm.logMonoTime['modelV2'] radarState.canMonoTimes = list(rr.canMonoTimes) radarState.radarErrors = list(rr.errors) radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] if enable_lead: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) + 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 @@ -187,7 +188,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): if can_sock is None: can_sock = messaging.sub_sock('can') if sm is None: - sm = messaging.SubMaster(['model', 'controlsState']) + sm = messaging.SubMaster(['modelV2', 'controlsState']) if pm is None: pm = messaging.PubMaster(['radarState', 'liveTracks']) diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index b49d039433..0919c126f6 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -106,7 +106,7 @@ int main(int argc, char **argv) { assert(err == 0); // messaging - PubMaster pm({"modelV2", "model", "cameraOdometry"}); + PubMaster pm({"modelV2", "cameraOdometry"}); SubMaster sm({"pathPlan", "frame"}); // cl init diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 91c09e0b86..bad5b3cce9 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,8 +10,6 @@ #include "driving.h" #include "clutil.h" -constexpr int MODEL_PATH_DISTANCE = 192; -constexpr int POLYFIT_DEGREE = 4; constexpr int DESIRE_PRED_SIZE = 32; constexpr int OTHER_META_SIZE = 4; @@ -32,7 +30,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); constexpr int POSE_SIZE = 12; -constexpr int MIN_VALID_LEN = 10; constexpr int PLAN_IDX = 0; constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; @@ -51,8 +48,6 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE; // #define DUMP_YUV -Eigen::Matrix vander; - void model_init(ModelState* s, cl_device_id device_id, cl_context context) { frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); s->input_frames = std::make_unique(MODEL_FRAME_SIZE * 2); @@ -75,13 +70,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) { s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN); #endif - // Build Vandermonde matrix - for(int i = 0; i < TRAJECTORY_SIZE; i++) { - for(int j = 0; j < POLYFIT_DEGREE - 1; j++) { - vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); - } - } - s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); } @@ -136,31 +124,6 @@ void model_free(ModelState* s) { CL_CHECK(clReleaseCommandQueue(s->q)); } -void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { - // References to inputs - Eigen::Map > pts(in_pts, valid_len); - Eigen::Map > std(in_stds, valid_len); - Eigen::Map > p(out, POLYFIT_DEGREE - 1); - - float y0 = pts[0]; - pts = pts.array() - y0; - - // Build Least Squares equations - Eigen::Matrix lhs = vander.topRows(valid_len).array().colwise() / std.array(); - Eigen::Matrix rhs = pts.array() / std.array(); - - // Improve numerical stability - Eigen::Matrix scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum(); - lhs = lhs * scale.asDiagonal(); - - // Solve inplace - p = lhs.colPivHouseholderQr().solve(rhs); - - // Apply scale to output - p = p.transpose() * scale.asDiagonal(); - out[3] = y0; -} - static const float *get_best_data(const float *data, int size, int group_size, int offset) { int max_idx = 0; for (int i = 1; i < size; i++) { @@ -180,30 +143,6 @@ static const float *get_lead_data(const float *lead, int t_offset) { return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION); } -void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob, - float valid_len, int valid_len_idx, int ll_idx) { - float points[TRAJECTORY_SIZE] = {}; - float stds[TRAJECTORY_SIZE] = {}; - float poly[POLYFIT_DEGREE] = {}; - - for (int i=0; i void fill_meta(MetaBuilder meta, const float *meta_data) { float desire_state_softmax[DESIRE_LEN]; @@ -332,54 +257,22 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou } } -void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) { - // Find the distribution that corresponds to the most probable plan - const float *best_plan = get_plan_data(net_outputs.plan); - // x pos at 10s is a good valid_len - float valid_len = 0; - for (int i=1; i= valid_len){ - valid_len = len; - } - } - // clamp to 10 and MODEL_PATH_DISTANCE - valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len)); - int valid_len_idx = 0; - for (int i=1; i= X_IDXS[valid_len_idx]){ - valid_len_idx = i; - } - } - fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0); - fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1); - fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2); - - fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0); - fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1); - - fill_meta(framed.initMeta(), net_outputs.meta); -} - void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop, const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof, float model_execution_time) { const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - auto do_publish = [&](auto init_model_func, const char *pub_name) { - MessageBuilder msg; - auto framed = (msg.initEvent().*(init_model_func))(); - framed.setFrameId(vipc_frame_id); - framed.setFrameAge(frame_age); - framed.setFrameDropPerc(frame_drop * 100); - framed.setTimestampEof(timestamp_eof); - framed.setModelExecutionTime(model_execution_time); - if (send_raw_pred) { - framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float))); - } - fill_model(framed, net_outputs); - pm.send(pub_name, msg); - }; - do_publish(&cereal::Event::Builder::initModel, "model"); - do_publish(&cereal::Event::Builder::initModelV2, "modelV2"); + MessageBuilder msg; + auto framed = msg.initEvent().initModelV2(); + framed.setFrameId(vipc_frame_id); + framed.setFrameAge(frame_age); + framed.setFrameDropPerc(frame_drop * 100); + framed.setTimestampEof(timestamp_eof); + framed.setModelExecutionTime(model_execution_time); + if (send_raw_pred) { + framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float))); + } + fill_model(framed, net_outputs); + pm.send("modelV2", msg); } void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1fd0353569..1eebbe4e61 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -12,7 +12,7 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['dMonitoringState']) if sm is None: - sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'model'], poll=['driverState']) + sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState']) driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1") @@ -44,8 +44,8 @@ def dmonitoringd_thread(sm=None, pm=None): driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise - if sm.updated['model']: - driver_status.set_policy(sm['model']) + if sm.updated['modelV2']: + driver_status.set_policy(sm['modelV2']) # Get data from dmonitoringmodeld events = Events() diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 504256b252..17a91c1db7 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -8,7 +8,7 @@ import numpy as np from opendbc import DBC_PATH -from cereal import car +from cereal import car, log from common.realtime import Ratekeeper from selfdrive.config import Conversions as CV import cereal.messaging as messaging @@ -111,10 +111,10 @@ class Plant(): if not Plant.messaging_initialized: - Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw', 'modelV2']) + Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw']) Plant.logcan = messaging.pub_sock('can') Plant.sendcan = messaging.sub_sock('sendcan') - Plant.model = messaging.pub_sock('model') + Plant.model = messaging.pub_sock('modelV2') Plant.live_params = messaging.pub_sock('liveParameters') Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.health = messaging.pub_sock('health') @@ -392,13 +392,9 @@ class Plant(): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step if publish_model and self.frame % 5 == 0: - md = messaging.new_message('model') + md = messaging.new_message('modelV2') cal = messaging.new_message('liveCalibration') - md.model.frameId = 0 - for x in [md.model.path, md.model.leftLane, md.model.rightLane]: - x.points = [0.0]*50 - x.prob = 1.0 - x.std = 1.0 + md.modelV2.frameId = 0 if self.lead_relevancy: d_rel = np.maximum(0., distance_lead - distance) @@ -409,15 +405,11 @@ class Plant(): v_rel = 0. prob = 0.0 - md.model.lead.dist = float(d_rel) - md.model.lead.prob = prob - md.model.lead.relY = 0.0 - md.model.lead.relYStd = 1. - md.model.lead.relVel = float(v_rel) - md.model.lead.relVelStd = 1. - md.model.lead.relA = 0.0 - md.model.lead.relAStd = 10. - md.model.lead.std = 1.0 + lead = log.ModelDataV2.LeadDataV2.new_message() + lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0] + lead.xyvaStd = [1.0, 1.0, 1.0, 1.0] + lead.prob = prob + md.modelV2.leads = [lead, lead] cal.liveCalibration.calStatus = 1 cal.liveCalibration.calPerc = 100 diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py index 69f34a665e..fe13a5cf66 100755 --- a/selfdrive/test/process_replay/camera_replay.py +++ b/selfdrive/test/process_replay/camera_replay.py @@ -35,7 +35,7 @@ def camera_replay(lr, fr, desire=None, calib=None): spinner.update("starting model replay") pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan']) - sm = messaging.SubMaster(['model', 'modelV2']) + sm = messaging.SubMaster(['modelV2']) # TODO: add dmonitoringmodeld print("preparing procs") @@ -74,7 +74,6 @@ def camera_replay(lr, fr, desire=None, calib=None): pm.send(msg.which(), f) with Timeout(seconds=15): - log_msgs.append(messaging.recv_one(sm.sock['model'])) log_msgs.append(messaging.recv_one(sm.sock['modelV2'])) spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count)) @@ -109,8 +108,9 @@ if __name__ == "__main__": log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit) cmp_log = LogReader(BASE_URL + log_fn) - ignore = ['logMonoTime', 'valid', 'model.frameDropPerc', 'model.modelExecutionTime', - 'modelV2.frameDropPerc', 'modelV2.modelExecutionTime'] + ignore = ['logMonoTime', 'valid', + 'modelV2.frameDropPerc', + 'modelV2.modelExecutionTime'] results: Any = {TEST_ROUTE: {}} results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, ignore_fields=ignore) diff1, diff2, failed = format_diff(results, ref_commit) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 29f307b4de..316467d247 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -222,7 +222,7 @@ CONFIGS = [ pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [], - "model": [], "frontFrame": [], "frame": [], "ubloxRaw": [], + "modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, @@ -233,7 +233,7 @@ CONFIGS = [ proc_name="radard", pub_sub={ "can": ["radarState", "liveTracks"], - "liveParameters": [], "controlsState": [], "model": [], + "liveParameters": [], "controlsState": [], "modelV2": [], }, ignore=["logMonoTime", "valid", "radarState.cumLagMs"], init_callback=get_car_params, @@ -266,7 +266,7 @@ CONFIGS = [ proc_name="dmonitoringd", pub_sub={ "driverState": ["dMonitoringState"], - "liveCalibration": [], "carState": [], "model": [], "controlsState": [], + "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [], }, ignore=["logMonoTime", "valid"], init_callback=get_car_params, diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 100ce7e370..e1d6502bfb 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3686b117b61bfe6ad49bdef435b9112869bbf7e0 \ No newline at end of file +0b75e9c24662151100e77057ff7d1ffcf55d841a \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 5a2d431717..854656a9b9 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -158,9 +158,6 @@ if __name__ == "__main__": if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ (not procs_whitelisted and cfg.proc_name in args.blacklist_procs): continue - # TODO remove this hack - if cfg.proc_name == 'plannerd' and car_brand in ["GM", "SUBARU", "VOLKSWAGEN", "NISSAN"]: - continue cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs)