bigmodel

commaai/openpilot#23684

Author: Moodkiller

selfdrive/controls/lib/lane_planner.py

@@ -27,7 +27,6 @@
 else:
   CAMERA_OFFSET = 0.0 + CameraOffset / 100
 
-
 # print("New CAMERA_OFFSET value in M is:", CAMERA_OFFSET)
 
 class LanePlanner:
@@ -53,26 +52,15 @@ def __init__(self, wide_camera=False):
     self.camera_offset = -CAMERA_OFFSET if wide_camera else CAMERA_OFFSET
     self.path_offset = -PATH_OFFSET if wide_camera else PATH_OFFSET
 
-#    self.CameraOffset = None
-#    self.CameraOffset = None
-
-#  def update_sunny_set_offsets(self, camera_offset, path_offset):
-#    if self.CameraOffset != camera_offset:
-#      self.CameraOffset = camera_offset
-#      self.camera_offset = camera_offset / 100
-
-#    if self.CameraOffset != path_offset:
-#      self.CameraOffset = path_offset
-#      self.path_offset = path_offset / 100
 
   def parse_model(self, md):
     if len(md.laneLines) == 4 and len(md.laneLines[0].t) == TRAJECTORY_SIZE:
       self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t))/2
       # left and right ll x is the same
       self.ll_x = md.laneLines[1].x
       # only offset left and right lane lines; offsetting path does not make sense
-      self.lll_y = np.array(md.laneLines[1].y) - self.camera_offset
-      self.rll_y = np.array(md.laneLines[2].y) - self.camera_offset
+      self.lll_y = np.array(md.laneLines[1].y) + self.camera_offset
+      self.rll_y = np.array(md.laneLines[2].y) + self.camera_offset
       self.lll_prob = md.laneLineProbs[1]
       self.rll_prob = md.laneLineProbs[2]
       self.lll_std = md.laneLineStds[1]
@@ -85,7 +73,7 @@ def parse_model(self, md):
   def get_d_path(self, v_ego, path_t, path_xyz):
     # Reduce reliance on lanelines that are too far apart or
     # will be in a few seconds
-    path_xyz[:, 1] -= self.path_offset
+    path_xyz[:, 1] += self.path_offset
     l_prob, r_prob = self.lll_prob, self.rll_prob
     width_pts = self.rll_y - self.lll_y
     prob_mods = []

selfdrive/modeld/SConscript

@@ -1,3 +1,5 @@
+import os
+
 Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc')
 lenv = env.Clone()
 
@@ -67,11 +69,14 @@ common_model = lenv.Object(common_src)
 if use_thneed and arch in ("aarch64", "larch64"):
   fn = "../../models/big_supercombo" if use_extra else "../../models/supercombo"
   compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
-  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} ../../models/supercombo.dlc ../../models/supercombo.thneed --binary"
+  cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}.thneed --binary"
 
   lib_paths = ':'.join(Dir(p).abspath for p in lenv["LIBPATH"])
-  cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}"})
-  cenv.Command("../../models/supercombo.thneed", ["../../models/supercombo.dlc", compiler], cmd)
+  kernel_path = os.path.join(Dir('.').abspath, "thneed", "kernels")
+  cenv = Environment(ENV={'LD_LIBRARY_PATH': f"{lib_paths}:{lenv['ENV']['LD_LIBRARY_PATH']}", 'KERNEL_PATH': kernel_path})
+
+  kernels = [os.path.join(kernel_path, x) for x in os.listdir(kernel_path) if x.endswith(".cl")]
+  cenv.Command(fn + ".thneed", [fn + ".dlc", kernels, compiler], cmd)
 
 lenv.Program('_dmonitoringmodeld', [
     "dmonitoringmodeld.cc",

selfdrive/modeld/models/driving.h

@@ -270,9 +270,8 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool
 ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
                               const mat3 &transform, const mat3 &transform_wide, float *desire_in);
 void model_free(ModelState* s);
-void poly_fit(float *in_pts, float *in_stds, float *out);
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
                    const ModelOutput &net_outputs, uint64_t timestamp_eof,
-                   float model_execution_time, kj::ArrayPtr<const float> raw_pred);
+                   float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
-                     const ModelOutput &net_outputs, uint64_t timestamp_eof);
+                     const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid);

selfdrive/modeld/runners/onnxmodel.cc

@@ -129,4 +129,3 @@ void ONNXModel::execute() {
   }
   pread(output, output_size);
 }
-

selfdrive/modeld/runners/onnxmodel.h

@@ -38,4 +38,3 @@ class ONNXModel : public RunModel {
   int pipein[2];
   int pipeout[2];
 };
-

selfdrive/modeld/runners/snpemodel.cc

@@ -239,4 +239,3 @@ void SNPEModel::execute() {
   }
 #endif
 }
-

selfdrive/modeld/runners/thneedmodel.cc

@@ -70,4 +70,3 @@ void ThneedModel::execute() {
     }
   }
 }
-

selfdrive/modeld/runners/thneedmodel.h

@@ -28,4 +28,3 @@ class ThneedModel : public RunModel {
   float *trafficConvention;
   float *desire;
 };
-

selfdrive/test/openpilotci.py

@@ -9,7 +9,7 @@
 
 def get_url(route_name, segment_num, log_type="rlog"):
   ext = "hevc" if log_type.endswith('camera') else "bz2"
-  return BASE_URL + f"{route_name.replace('|', '/')}/{segment_num}/{log_type}.{ext}"
+  return BASE_URL + "%s/%s/%s.%s" % (route_name.replace("|", "/"), segment_num, log_type, ext)
 
 def upload_file(path, name):
   from azure.storage.blob import BlockBlobService  # pylint: disable=import-error

selfdrive/test/process_replay/model_replay.py

@@ -6,14 +6,12 @@
 from typing import Any
 from itertools import zip_longest
 
-from tqdm import tqdm
-
 import cereal.messaging as messaging
 from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType  # pylint: disable=no-name-in-module, import-error
 from common.spinner import Spinner
 from common.timeout import Timeout
 from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \
-                                                                        eon_d_frame_size, tici_d_frame_size
+                                          eon_d_frame_size, tici_d_frame_size
 from selfdrive.hardware import PC, TICI
 from selfdrive.manager.process_config import managed_processes
 from selfdrive.test.openpilotci import BASE_URL, get_url
@@ -27,51 +25,25 @@
   TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
 else:
   TEST_ROUTE = "303055c0002aefd1|2021-11-22--18-36-32"
+SEGMENT = 0
 
-CACHE_DIR = os.getenv("CACHE_DIR", None)
+SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
 
 VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
                "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD}
 
 def get_log_fn(ref_commit):
-  return "%s_%s_%s.bz2" % (TEST_ROUTE, "model_tici" if TICI else "model", ref_commit)
+  return f"{TEST_ROUTE}_{'model_tici' if TICI else 'model'}_{ref_commit}.bz2"
+
 
 def replace_calib(msg, calib):
   msg = msg.as_builder()
   if calib is not None:
     msg.liveCalibration.extrinsicMatrix = get_view_frame_from_road_frame(*calib, 1.22).flatten().tolist()
   return msg
 
-def process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire):
-  if msg.which() == "roadCameraState" and last_desire is not None:
-    dat = messaging.new_message('lateralPlan')
-    dat.lateralPlan.desire = last_desire
-    pm.send('lateralPlan', dat)
-
-  f = msg.as_builder()
-  pm.send(msg.which(), f)
-
-  img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0]
-  if msg.which == "roadCameraState":
-    vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, img.flatten().tobytes(), f.roadCameraState.frameId,
-                      f.roadCameraState.timestampSof, f.roadCameraState.timestampEof)
-  else:
-    vipc_server.send(VisionStreamType.VISION_STREAM_DRIVER, img.flatten().tobytes(), f.driverCameraState.frameId,
-                      f.driverCameraState.timestampSof, f.driverCameraState.timestampEof)
-  with Timeout(seconds=15):
-    log_msgs.append(messaging.recv_one(sm.sock[packet_from_camera[msg.which()]]))
-
-  frame_idxs[msg.which()] += 1
-  if frame_idxs[msg.which()] >= frs[msg.which()].frame_count:
-    return None
-  update_spinner(spinner, frame_idxs['roadCameraState'], frs['roadCameraState'].frame_count,
-                                         frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)
-  return 0
-
-def update_spinner(s, fidx, fcnt, didx, dcnt):
-  s.update("replaying models:  road %d/%d,  driver %d/%d" % (fidx, fcnt, didx, dcnt))
-
-def model_replay(lr_list, frs):
+
+def model_replay(lr, frs):
   spinner = Spinner()
   spinner.update("starting model replay")
 
@@ -90,13 +62,16 @@ def model_replay(lr_list, frs):
     time.sleep(5)
     sm.update(1000)
 
+    log_msgs = []
     last_desire = None
     recv_cnt = defaultdict(lambda: 0)
     frame_idxs = defaultdict(lambda: 0)
 
-    cal = [msg for msg in lr if msg.which() == "liveCalibration"]
-    for msg in cal[:5]:
-      pm.send(msg.which(), replace_calib(msg, None))
+    # init modeld with valid calibration
+    cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"]
+    for _ in range(5):
+      pm.send(cal_msgs[0].which(), cal_msgs[0].as_builder())
+      time.sleep(0.1)
 
     msgs = defaultdict(list)
     for msg in lr:
@@ -157,15 +132,13 @@ def model_replay(lr_list, frs):
     managed_processes['modeld'].stop()
     managed_processes['dmonitoringmodeld'].stop()
 
+
   return log_msgs
 
+
 if __name__ == "__main__":
 
   update = "--update" in sys.argv
-
-  if TICI:
-    os.system('sudo mount -o remount,size=200M /tmp') # c3 hevcs are 75M each
-
   replay_dir = os.path.dirname(os.path.abspath(__file__))
   ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
 
@@ -178,12 +151,14 @@ def model_replay(lr_list, frs):
   if TICI:
     frs['wideRoadCameraState'] = FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"))
 
-  lr_list = list(lr)
-  log_msgs = model_replay(lr_list, frs)
+  # run replay
+  log_msgs = model_replay(lr, frs)
 
+  # get diff
   failed = False
   if not update:
-    ref_commit = open(ref_commit_fn).read().strip()
+    with open(ref_commit_fn) as f:
+      ref_commit = f.read().strip()
     log_fn = get_log_fn(ref_commit)
     try:
       cmp_log = LogReader(BASE_URL + log_fn)
@@ -209,6 +184,7 @@ def model_replay(lr_list, frs):
       print(str(e))
       failed = True
 
+  # upload new refs
   if update or failed:
     from selfdrive.test.openpilotci import upload_file