diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 6e7fd26eb1..18410584b9 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b5bca4529ca5c2d6c12f2faa48265f706c057ec1d1243587fc8e4f31233bdf94 -size 27183806 +oid sha256:e1094ebfbe14692cdfe9645aedd36680ed83b40a7afe0fc5d3943d7a2df0ee9a +size 26664761 diff --git a/models/supercombo.keras b/models/supercombo.keras index fc4503eba3..c96e2646e2 100644 --- a/models/supercombo.keras +++ b/models/supercombo.keras @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7370937c2e9f271273bb2d2f8a41e574eda3f347d9926c3f91d9894828cb2999 -size 28035608 +oid sha256:c7b36f2273a3f2f5b418642d2aa2b4c39a9a9c6579e2c88ec9b1615038f7751f +size 27524584 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 680e511e01..1d4118eb42 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -61,6 +61,8 @@ def events_to_bytes(events): for e in events: if isinstance(e, capnp.lib.capnp._DynamicStructReader): e = e.as_builder() + if not e.is_root: + e = e.copy() ret.append(e.to_bytes()) return ret diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 0e84ad8b9f..ff090bcd97 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -65,9 +65,9 @@ class LanePlanner(): self.l_prob = md.leftLane.prob # left line prob self.r_prob = md.rightLane.prob # right line prob - if len(md.meta.desirePrediction): - self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] - self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + if len(md.meta.desireState): + self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] + self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 83b1190fff..cee66c6e87 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -22,19 +22,16 @@ DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, }, LaneChangeDirection.left: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, }, } @@ -119,14 +116,10 @@ class PathPlanner(): if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: - self.lane_change_state = LaneChangeState.laneChangeStarting - - # starting - elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: - self.lane_change_state = LaneChangeState.laneChangeFinishing + self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: + elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.5: if one_blinker: self.lane_change_state = LaneChangeState.preLaneChange else: diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 259aafff4e..b544a3e59c 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,10 +10,11 @@ #define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1 #define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2 #define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION +#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION #define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 #define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 -#define META_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL @@ -43,7 +44,9 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #ifdef DESIRE s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; - s->m->addDesire(s->desire, DESIRE_SIZE); + s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); + for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0; + s->m->addDesire(s->pulse_desire, DESIRE_SIZE); #endif // Build Vandermonde matrix @@ -61,7 +64,16 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, mat3 transform, void* sock, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i]; + for (int i = 0; i < DESIRE_SIZE; i++) { + // Model decides when action is completed + // so desire input is just a pulse triggered on rising edge + if (desire_in[i] - s->desire[i] == 1) { + s->pulse_desire[i] = desire_in[i]; + } else { + s->pulse_desire[i] = 0.0; + } + s->desire[i] = desire_in[i]; + } } #endif @@ -88,7 +100,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, net_outputs.long_x = &s->output[LONG_X_IDX]; net_outputs.long_v = &s->output[LONG_V_IDX]; net_outputs.long_a = &s->output[LONG_A_IDX]; - net_outputs.meta = &s->output[META_IDX]; + net_outputs.meta = &s->output[DESIRE_STATE_IDX]; net_outputs.pose = &s->output[POSE_IDX]; return net_outputs; } @@ -183,11 +195,13 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, in } void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { - meta.setEngagedProb(meta_data[0]); - meta.setGasDisengageProb(meta_data[1]); - meta.setBrakeDisengageProb(meta_data[2]); - meta.setSteerOverrideProb(meta_data[3]); - kj::ArrayPtr desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE); + kj::ArrayPtr desire_state(&meta_data[0], DESIRE_LEN); + meta.setDesireState(desire_state); + meta.setEngagedProb(meta_data[DESIRE_LEN]); + meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]); + meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]); + meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]); + kj::ArrayPtr desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE); meta.setDesirePrediction(desire_pred); } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index bf46cb9d47..742eb6cedb 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -34,6 +34,7 @@ #define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 +#define DESIRE_LEN 8 #define DESIRE_PRED_SIZE 32 #define OTHER_META_SIZE 4 #define LEAD_MDN_N 5 // probs for 5 groups @@ -51,6 +52,7 @@ struct ModelDataRaw { float *long_x; float *long_v; float *long_a; + float *desire_state; float *meta; float *pose; }; @@ -63,6 +65,7 @@ typedef struct ModelState { RunModel *m; #ifdef DESIRE float *desire; + float *pulse_desire; #endif } ModelState; diff --git a/selfdrive/test/process_replay/inject_model.py b/selfdrive/test/process_replay/inject_model.py new file mode 100755 index 0000000000..08fd74a621 --- /dev/null +++ b/selfdrive/test/process_replay/inject_model.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 + +import os +import time + + +from tqdm import tqdm +from cereal.messaging import PubMaster, recv_one, sub_sock +from tools.lib.framereader import FrameReader +import subprocess +import selfdrive.manager as manager + + +def rreplace(s, old, new, occurrence): + li = s.rsplit(old, occurrence) + return new.join(li) + + +def regen_model(msgs, pm, frame_reader, model_sock): + # Send some livecalibration messages to initalize visiond + for msg in msgs: + if msg.which() == 'liveCalibration': + pm.send('liveCalibration', msg.as_builder()) + + + out_msgs = [] + fidx = 0 + for msg in tqdm(msgs): + w = msg.which() + + if w == 'frame': + msg = msg.as_builder() + + img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:,::-1] + + msg.frame.image = img.flatten().tobytes() + + pm.send(w, msg) + model = recv_one(model_sock) + fidx += 1 + out_msgs.append(model) + elif w == 'liveCalibration': + pm.send(w, msg.as_builder()) + + return out_msgs + + +def inject_model(msgs, segment_name): + if segment_name.count('--') == 2: + segment_name = rreplace(segment_name, '--', '/', 1) + frame_reader = FrameReader('cd:/'+segment_name.replace("|", "/") + "/fcamera.hevc") + + manager.start_managed_process('camerad') + manager.start_managed_process('modeld') + # TODO do better than just wait for modeld to boot + time.sleep(5) + + pm = PubMaster(['liveCalibration', 'frame']) + model_sock = sub_sock('model') + try: + out_msgs = regen_model(msgs, pm, frame_reader, model_sock) + except (KeyboardInterrupt, SystemExit, Exception) as e: + manager.kill_managed_process('modeld') + time.sleep(2) + manager.kill_managed_process('camerad') + raise e + manager.kill_managed_process('modeld') + time.sleep(2) + manager.kill_managed_process('camerad') + + + new_msgs = [] + midx = 0 + for msg in msgs: + if (msg.which() == 'model') and (midx < len(out_msgs)): + model = out_msgs[midx].as_builder() + model.logMonoTime = msg.logMonoTime + model = model.as_reader() + new_msgs.append(model) + midx += 1 + else: + new_msgs.append(msg) + + print(len(new_msgs), len(list(msgs))) + assert abs(len(new_msgs) - len(list(msgs))) < 2 + + return new_msgs + + + +if __name__ == "__main__": + inject_model("0375fdf7b1ce594d|2019-06-13--08-32-25/3") diff --git a/selfdrive/test/process_replay/model_ref_commit b/selfdrive/test/process_replay/model_ref_commit new file mode 100644 index 0000000000..7e0a58e2b0 --- /dev/null +++ b/selfdrive/test/process_replay/model_ref_commit @@ -0,0 +1 @@ +95638846316e5de7f0314ed2330b01428792c889 \ No newline at end of file diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7edbcb9fb7..57aba612f9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a8f2dbe727e8b999a4e1df024abf919c35b1ac7d \ No newline at end of file +43b4e291bb92ee02066c59a13ef28aa900a3f092 diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 94d831ac03..54eae7c27d 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -6,19 +6,23 @@ import sys import tempfile from selfdrive.car.car_helpers import interface_names -from selfdrive.test.process_replay.compare_logs import compare_logs from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS +from selfdrive.test.process_replay.compare_logs import compare_logs from tools.lib.logreader import LogReader + +INJECT_MODEL = 0 + segments = [ ("HONDA", "0375fdf7b1ce594d|2019-06-13--08-32-25--3"), # HONDA.ACCORD ("HONDA", "99c94dc769b5d96e|2019-08-03--14-19-59--2"), # HONDA.CIVIC - ("TOYOTA", "cce908f7eb8db67d|2019-08-02--15-09-51--3"), # TOYOTA.COROLLA_TSS2 - ("GM", "7ad88f53d406b787|2019-07-09--10-18-56--8"), # GM.VOLT - ("HYUNDAI", "704b2230eb5190d6|2019-07-06--19-29-10--0"), # HYUNDAI.KIA_SORENTO - ("CHRYSLER", "b6e1317e1bfbefa6|2019-07-06--04-05-26--5"), # CHRYSLER.JEEP_CHEROKEE + ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2 + ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA + ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA + ("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"), # HYUNDAI.SANTA_FE + #("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE ("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA - ("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"), # VW.GOLF + ("VOLKSWAGEN", "b0c9d2329ad1606b|2020-02-19--16-29-36--7"), # VW.GOLF ] # ford doesn't need to be tested until a full port is done @@ -29,9 +33,14 @@ BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" # run the full test (including checks) when no args given FULL_TEST = len(sys.argv) <= 1 -def get_segment(segment_name): +def get_segment(segment_name, original=True): route_name, segment_num = segment_name.rsplit("--", 1) - rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num) + if original: + rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num) + else: + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + model_ref_commit = open(os.path.join(process_replay_dir, "model_ref_commit")).read().strip() + rlog_url = BASE_URL + "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, model_ref_commit) req = requests.get(rlog_url) assert req.status_code == 200, ("Failed to download log for %s" % segment_name) diff --git a/selfdrive/test/process_replay/update_model.py b/selfdrive/test/process_replay/update_model.py new file mode 100755 index 0000000000..d3dab335ef --- /dev/null +++ b/selfdrive/test/process_replay/update_model.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import os +import sys + +from selfdrive.test.openpilotci_upload import upload_file +from selfdrive.test.process_replay.compare_logs import save_log +from selfdrive.test.process_replay.test_processes import segments, get_segment +from selfdrive.version import get_git_commit +from tools.lib.logreader import LogReader +from inject_model import inject_model + +if __name__ == "__main__": + + no_upload = "--no-upload" in sys.argv + + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(process_replay_dir, "model_ref_commit") + + ref_commit = get_git_commit() + with open(ref_commit_fn, "w") as f: + f.write(ref_commit) + + for car_brand, segment in segments: + rlog_fn = get_segment(segment, original=True) + + if rlog_fn is None: + print("failed to get segment %s" % segment) + sys.exit(1) + + lr = LogReader(rlog_fn) + print('injecting model into % s' % segment) + lr = inject_model(lr, segment) + + route_name, segment_num = segment.rsplit("--", 1) + log_fn = "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, ref_commit) + tmp_name = 'tmp_%s_%s' % (route_name, segment_num) + save_log(tmp_name, lr) + + if not no_upload: + upload_file(tmp_name, log_fn) + print('uploaded %s', log_fn) + os.remove(tmp_name) + os.remove(rlog_fn) + + print("done")