Log MPC solver time (#23566)

* add solve time to plan output

* add to process replay ignore list

* print in CI

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -3,6 +3,8 @@ import os
 import numpy as np
 
 from casadi import SX, vertcat, sin, cos
+
+from common.realtime import sec_since_boot
 from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
 from selfdrive.controls.lib.drive_helpers import T_IDXS
 
@@ -132,6 +134,7 @@ class LateralMpc():
     self.solver.constraints_set(0, "ubx", x0)
     self.solver.solve()
     self.solution_status = 0
+    self.solve_time = 0.0
     self.cost = 0
 
   def set_weights(self, path_weight, heading_weight, steer_rate_weight):
@@ -151,7 +154,10 @@ class LateralMpc():
       self.solver.cost_set(i, "yref", self.yref[i])
     self.solver.cost_set(N, "yref", self.yref[N][:2])
 
+    t = sec_since_boot()
     self.solution_status = self.solver.solve()
+    self.solve_time = sec_since_boot() - t
+
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
     for i in range(N):

selfdrive/controls/lib/lateral_planner.py

@@ -215,6 +215,7 @@ class LateralPlanner:
     lateralPlan.dProb = float(self.LP.d_prob)
 
     lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
+    lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
 
     lateralPlan.desire = self.desire
     lateralPlan.useLaneLines = self.use_lanelines

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -215,6 +215,7 @@ class LongitudinalMpc:
     self.status = False
     self.crash_cnt = 0.0
     self.solution_status = 0
+    self.solve_time = 0.0
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
@@ -356,7 +357,11 @@ class LongitudinalMpc:
       self.solver.set(i, 'p', self.params[i])
     self.solver.constraints_set(0, "lbx", self.x0)
     self.solver.constraints_set(0, "ubx", self.x0)
+
+    t = sec_since_boot()
     self.solution_status = self.solver.solve()
+    self.solve_time = sec_since_boot() - t
+
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
     for i in range(N):
@@ -368,7 +373,6 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
-    t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t

selfdrive/controls/lib/longitudinal_planner.py

@@ -120,4 +120,6 @@ class Planner:
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
+
     pm.send('longitudinalPlan', plan_send)

selfdrive/test/process_replay/process_replay.py

@@ -267,7 +267,7 @@ CONFIGS = [
       "modelV2": ["lateralPlan", "longitudinalPlan"],
       "carState": [], "controlsState": [], "radarState": [],
     },
-    ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"],
+    ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"],
     init_callback=get_car_params,
     should_recv_callback=None,
     tolerance=NUMPY_TOLERANCE,

selfdrive/test/test_onroad.py

@@ -204,6 +204,22 @@ class TestOnroad(unittest.TestCase):
     cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1])
     self.assertTrue(cpu_ok)
 
+  def test_mpc_execution_timings(self):
+    result = "\n"
+    result += "------------------------------------------------\n"
+    result += "-----------------  MPC Timing ------------------\n"
+    result += "------------------------------------------------\n"
+
+    cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)]
+    for (s, instant_max, avg_max) in cfgs:
+      ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s]
+      self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
+      self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
+      result += f"'{s}' execution time: {min(ts)}\n"
+      result += f"'{s}' avg execution time: {np.mean(ts)}\n"
+    result += "------------------------------------------------\n"
+    print(result)
+
   def test_model_execution_timings(self):
     result = "\n"
     result += "------------------------------------------------\n"