Pulse desire (#1172)

* initial

* compilation fixes

* add desire

* pulse desire

* 0d5728f6-a918-40a6-bce3-a9d0f1238180/20

* cleaner

* inject model

* inject model

* test runs

* ah already done I see

* remove useless

* update

* updates, doesnt work

* fix generating new references

Co-authored-by: Willem Melching <willem.melching@gmail.com>

models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:b5bca4529ca5c2d6c12f2faa48265f706c057ec1d1243587fc8e4f31233bdf94
+oid sha256:e1094ebfbe14692cdfe9645aedd36680ed83b40a7afe0fc5d3943d7a2df0ee9a
-size 27183806
+size 26664761

models/supercombo.keras

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7370937c2e9f271273bb2d2f8a41e574eda3f347d9926c3f91d9894828cb2999
+oid sha256:c7b36f2273a3f2f5b418642d2aa2b4c39a9a9c6579e2c88ec9b1615038f7751f
-size 28035608
+size 27524584

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
 

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):
+    if len(md.meta.desireState):
-      self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
+      self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1]
-      self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 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

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
+          self.lane_change_state = LaneChangeState.laneChangeFinishing
-
-      # starting
-      elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
-        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:

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]);
+  kj::ArrayPtr<const float> desire_state(&meta_data[0], DESIRE_LEN);
-  meta.setGasDisengageProb(meta_data[1]);
+  meta.setDesireState(desire_state);
-  meta.setBrakeDisengageProb(meta_data[2]);
+  meta.setEngagedProb(meta_data[DESIRE_LEN]);
-  meta.setSteerOverrideProb(meta_data[3]);
+  meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]);
-  kj::ArrayPtr<const float> desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE);
+  meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]);
+  meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]);
+  kj::ArrayPtr<const float> desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE);
   meta.setDesirePrediction(desire_pred);
 }
 

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;
 

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")

selfdrive/test/process_replay/model_ref_commit

@@ -0,0 +1 @@
+95638846316e5de7f0314ed2330b01428792c889

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-a8f2dbe727e8b999a4e1df024abf919c35b1ac7d
+43b4e291bb92ee02066c59a13ef28aa900a3f092

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
+  ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"),     # TOYOTA.COROLLA_TSS2
-  ("GM", "7ad88f53d406b787|2019-07-09--10-18-56--8"),         # GM.VOLT
+  ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"),         # GM.ACADIA
-  ("HYUNDAI", "704b2230eb5190d6|2019-07-06--19-29-10--0"),    # HYUNDAI.KIA_SORENTO
+  ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"),   # CHRYSLER.PACIFICA
-  ("CHRYSLER", "b6e1317e1bfbefa6|2019-07-06--04-05-26--5"),   # CHRYSLER.JEEP_CHEROKEE
+  ("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)
 

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")