lateral_planner: expose solverState, solverCost in debug mode (#28577)

* Update cereal

* Add xs, ys, solverCost support in lat planner

* Update cereal

* Add support for debug mode. Add solverState

* Read debug mode from env

* Update cereal
old-commit-hash: 83d6552a15

selfdrive/controls/lib/lateral_planner.py

@@ -25,7 +25,7 @@ STEERING_RATE_COST = 700.0
 
 
 class LateralPlanner:
-  def __init__(self, CP):
+  def __init__(self, CP, debug=False):
     self.DH = DesireHelper()
 
     # Vehicle model parameters used to calculate lateral movement of car
@@ -45,6 +45,8 @@ class LateralPlanner:
     self.l_lane_change_prob = 0.0
     self.r_lane_change_prob = 0.0
 
+    self.debug_mode = debug
+
     self.lat_mpc = LateralMpc()
     self.reset_mpc(np.zeros(4))
 
@@ -132,6 +134,11 @@ class LateralPlanner:
 
     lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
     lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
+    if self.debug_mode:
+      lateralPlan.solverCost = self.lat_mpc.cost
+      lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
+      lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
+      lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
 
     lateralPlan.desire = self.DH.desire
     lateralPlan.useLaneLines = False

selfdrive/controls/plannerd.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import os
 import numpy as np
 from cereal import car
 from common.params import Params
@@ -34,8 +35,10 @@ def plannerd_thread(sm=None, pm=None):
   CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
+  debug_mode = bool(int(os.getenv("DEBUG", "0")))
+
   longitudinal_planner = LongitudinalPlanner(CP)
-  lateral_planner = LateralPlanner(CP)
+  lateral_planner = LateralPlanner(CP, debug=debug_mode)
 
   if sm is None:
     sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],