diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 6162cf68a51ae3..eb56e201773767 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -3,6 +3,8 @@ 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 @@ def reset(self, x0=np.zeros(X_DIM)): 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 @@ def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts): 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): diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 110c6def67f1d0..7ff4fc954e5ef7 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -215,6 +215,7 @@ def publish(self, sm, pm): 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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c07110eefad3cf..8625df745abd25 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -215,6 +215,7 @@ def reset(self): 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 @@ def run(self): 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 @@ def run(self): 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9dc6ab9ee5e1ec..8c79404058f2eb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -120,4 +120,6 @@ def publish(self, sm, pm): longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw + longitudinalPlan.solverExecutionTime = self.mpc.solve_time + pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 8a13e4b18e1edf..ea762a36cb5cbc 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -267,7 +267,7 @@ def ublox_rcv_callback(msg): "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, diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 2f85605b8335bc..1432a1e5b07cf7 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -204,6 +204,22 @@ def test_cpu_usage(self): 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"