e2e fix

long_debug.py

@@ -0,0 +1,26 @@
+from cereal.messaging import SubMaster
+
+sm = SubMaster(['modelV2'], poll=['modelV2'])
+model_t = [0, 0.009765625, 0.0390625, 0.087890625, 0.15625, 0.24414062, 0.3515625, 0.47851562, 0.625, 0.79101562, 0.9765625, 1.1816406, 1.40625, 1.6503906, 1.9140625, 2.1972656, 2.5, 2.8222656, 3.1640625, 3.5253906, 3.90625, 4.3066406, 4.7265625, 5.1660156, 5.625, 6.1035156, 6.6015625, 7.1191406, 7.65625, 8.2128906, 8.7890625, 9.3847656, 10]
+mpc_idxs = list(range(10))
+
+model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_idxs]  # matches 0 to 9 interval to idx from t
+# speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0]  # idx used for current speed, position still uses model_t_idx
+
+
+while 1:
+  sm.update()
+
+  modelV2 = sm['modelV2']
+  if not sm.updated['modelV2'] or len(modelV2.position.x) == 0:
+    continue
+
+  distances, speeds, accelerations = [], [], []  # everything is derived from x position since velocity is outputting weird values
+  for t in model_t_idx:
+    speeds.append(modelV2.velocity.x[t])
+    distances.append(modelV2.position.x[t])
+    if model_t_idx.index(t) > 0:  # skip first since we can't calculate (and don't want to use v_ego)
+      accelerations.append((speeds[-1] - speeds[-2]) / model_t[t])
+
+  accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1]))  # extrapolate back first accel from second and third, less weight
+  print([round(i * 2.23694, 2) for i in speeds[:5]])

selfdrive/controls/lib/planner.py

@@ -118,27 +118,18 @@ def parse_modelV2_data(self, sm):
       return distances, speeds, accelerations
 
     model_t = modelV2.position.t
-    mpc_times = [0.009765625] + list(range(1, 10))
+    mpc_times = list(range(10))
 
     model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times]  # matches 0 to 9 interval to idx from t
-    speed_curr_idx = sorted(range(len(model_t)), key=[abs(t - .1) for t in model_t].__getitem__)[0]  # idx used for current speed, position still uses model_t_idx
 
-    distances = []  # everything is derived from x position since velocity is outputting weird values
-    speeds = []
-    accelerations = [0]
-    for t in model_t_idx:
+    for t in model_t_idx:  # everything is derived from x position since velocity is outputting weird values
+      speeds.append(modelV2.velocity.x[t])
       distances.append(modelV2.position.x[t])
-
-      if t == 0:
-        speeds.append(modelV2.position.x[speed_curr_idx] / model_t[speed_curr_idx])
-      else:
-        speeds.append(modelV2.position.x[t] / model_t[t])
-
       if model_t_idx.index(t) > 0:  # skip first since we can't calculate (and don't want to use v_ego)
         accelerations.append((speeds[-1] - speeds[-2]) / model_t[t])
 
-    accelerations[0] = accelerations[1] - (accelerations[2] - accelerations[1])  # extrapolate back first accel from second and third, less weight
-    return distances, speeds, accelerations  # hope this works
+    accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1]))  # extrapolate back first accel from second and third, less weight
+    return distances, speeds, accelerations
 
   def update(self, sm, pm, CP, VM, PP):
     """Gets called when new radarState is available"""

selfdrive/debug/uiview.py

@@ -1,11 +1,15 @@
 #!/usr/bin/env python3
+import os
 import time
+import signal
+import subprocess
 import cereal.messaging as messaging
-from selfdrive.manager import start_managed_process, kill_managed_process
+from common.basedir import BASEDIR
 
 services = ['controlsState', 'thermal', 'radarState']  # the services needed to be spoofed to start ui offroad
-procs = ['camerad', 'ui', 'modeld', 'calibrationd']
-[start_managed_process(p) for p in procs]  # start needed processes
+procs = {'camerad': 'selfdrive/camerad/camerad', 'ui': 'selfdrive/ui/ui',
+         'modeld': 'selfdrive/modeld/modeld', 'calibrationd': 'selfdrive/locationd/calibrationd.py'}
+started_procs = [subprocess.Popen(os.path.join(BASEDIR, procs[p]), cwd=os.path.join(BASEDIR, os.path.dirname(procs[p]))) for p in procs]  # start needed processes
 pm = messaging.PubMaster(services)
 
 dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services]
@@ -17,6 +21,6 @@
     pm.send('controlsState', dat_cs)
     pm.send('thermal', dat_thermal)
     pm.send('radarState', dat_radar)
-    time.sleep(1 / 100)  # continually send, rate doesn't matter for thermal
+    time.sleep(1 / 100)  # continually send, rate doesn't matter
 except KeyboardInterrupt:
-  [kill_managed_process(p) for p in procs]
+  [p.send_signal(signal.SIGINT) for p in started_procs]

selfdrive/manager.py

@@ -89,7 +89,7 @@ def unblock_stdout():
     env['SCONS_CACHE'] = "1"
 
     nproc = os.cpu_count()
-    j_flag = "" if nproc is None else "-j%d" % (nproc - 1)
+    j_flag = "" if nproc is None else "-j8"
     scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
 
     compile_output = []
@@ -124,8 +124,8 @@ def unblock_stdout():
           for i in range(3, -1, -1):
             print("....%d" % i)
             time.sleep(1)
-          subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
-          shutil.rmtree("/tmp/scons_cache")
+          # subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
+          # shutil.rmtree("/tmp/scons_cache")
         else:
           print("scons build failed after retry")
           sys.exit(1)

selfdrive/modeld/models/driving.cc

@@ -325,14 +325,6 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
       plan_mhp_max_idx = i;
     }
   }
-  float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
-  valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
-  int valid_len_idx = 0;
-  for (int i=1; i<TRAJECTORY_SIZE; i++) {
-    if (valid_len >= X_IDXS[valid_len_idx]){
-      valid_len_idx = i;
-    }
-  }
 
   float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
   float plan_t_arr[TRAJECTORY_SIZE];
@@ -342,12 +334,12 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   auto position = framed.initPosition();
   fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
-  auto orientation = framed.initOrientation();
-  fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
   auto velocity = framed.initVelocity();
-  fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 6, plan_t_arr);
+  fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
+  auto orientation = framed.initOrientation();
+  fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
   auto orientation_rate = framed.initOrientationRate();
-  fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
+  fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
 
   // lane lines
   auto lane_lines = framed.initLaneLines(4);
@@ -411,9 +403,17 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
       plan_mhp_max_idx = i;
     }
   }
+
   // x pos at 10s is a good valid_len
-  float valid_len = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*32];
-  // clamp to 5 and MODEL_PATH_DISTANCE
+  float valid_len = 0;
+  float valid_len_candidate;
+  for (int i=1; i<TRAJECTORY_SIZE; i++) {
+    valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
+    if (valid_len_candidate >= valid_len){
+      valid_len = valid_len_candidate;
+    }
+  }
+  // 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<TRAJECTORY_SIZE; i++) {