Move personality to controlsState (#31855)

* start at param

* start by sending personality

* change to personality

* POC: button changes personality

* what's wrong with this?

* fix

* not really possible but fuzzy test catches this

* there's always a typo

* dang, we're dropping messages

* clean up

* no comment

* bump

* rename

* not all cars yet

* works but at what cost

* clean up

* inside settings

* write param so we save the distance button changes

* setChecked activates buttonToggled and already writes param!

* don't need this, we update from longitudinalPlan on changes

* some clean up

* more

* ui

* allow some time for ui to receive and write param

* plannerd: only track changes in case no ui

* Revert "plannerd: only track changes in case no ui"

This reverts commit 2b081aa.

* write in plannerd as well, I assume this is atomic?

* don't write when setting checked (only user clicks)

* better nane

* more

* Update selfdrive/controls/lib/longitudinal_planner.py

Co-authored-by: Cameron Clough <cameronjclough@gmail.com>

* doesn't write param now

* ParamWatcher is nice

* no debug

* Update translations

* fix

* odd drain sock proc replay behavior

* vanish

* Revert "odd drain sock proc replay behavior"

This reverts commit 29b70b3.

* add GM

* only if OP long

* move personality to controlsState, since eventually it won't be exclusive to long planner

more

bump

* diff without translations

* fix

* put nonblocking

* CS should start at up to date personality always (no ui flicker)

* update toggle on cereal message change

* fix

* fix that

* ubmp

* mypy doesn't know this is an int :(

* update translations

* fix the tests

* revert ui

* not here

* migrate controlsState

* Revert "migrate controlsState" - i see no reason we need to test with
any specific personality

This reverts commit 6063508.

* Update ref_commit

---------

Co-authored-by: Cameron Clough <cameronjclough@gmail.com>

selfdrive/controls/controlsd.py

@@ -146,6 +146,7 @@ def __init__(self, CI=None):
     self.steer_limited = False
     self.desired_curvature = 0.0
     self.experimental_mode = False
+    self.personality = self.read_personality_param()
     self.v_cruise_helper = VCruiseHelper(self.CP)
     self.recalibrating_seen = False
 
@@ -680,7 +681,7 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     hudControl.speedVisible = self.enabled
     hudControl.lanesVisible = self.enabled
     hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
-    hudControl.leadDistanceBars = self.sm['longitudinalPlan'].personality.raw + 1
+    hudControl.leadDistanceBars = self.personality + 1
 
     hudControl.rightLaneVisible = True
     hudControl.leftLaneVisible = True
@@ -769,6 +770,7 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     controlsState.forceDecel = bool(force_decel)
     controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
     controlsState.experimentalMode = self.experimental_mode
+    controlsState.personality = self.personality
 
     lat_tuning = self.CP.lateralTuning.which()
     if self.joystick_mode:
@@ -821,10 +823,17 @@ def step(self):
 
     self.CS_prev = CS
 
+  def read_personality_param(self):
+    try:
+      return int(self.params.get('LongitudinalPersonality'))
+    except (ValueError, TypeError):
+      return log.LongitudinalPersonality.standard
+
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      self.personality = self.read_personality_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
       time.sleep(0.1)

selfdrive/controls/lib/longitudinal_planner.py

@@ -2,8 +2,6 @@
 import math
 import numpy as np
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.params import Params
-from cereal import log
 
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
@@ -61,16 +59,6 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
-    self.params = Params()
-    self.param_read_counter = 0
-    self.personality = log.LongitudinalPersonality.standard
-    self.read_param()
-
-  def read_param(self):
-    try:
-      self.personality = int(self.params.get('LongitudinalPersonality'))
-    except (ValueError, TypeError):
-      self.personality = log.LongitudinalPersonality.standard
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -89,9 +77,6 @@ def parse_model(model_msg, model_error):
     return x, v, a, j
 
   def update(self, sm):
-    if self.param_read_counter % 50 == 0:
-      self.read_param()
-    self.param_read_counter += 1
     self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
@@ -130,11 +115,11 @@ def update(self, sm):
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
-    self.mpc.set_weights(prev_accel_constraint, personality=self.personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
 
     self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
@@ -170,6 +155,5 @@ def publish(self, sm, pm):
     longitudinalPlan.fcw = self.fcw
 
     longitudinalPlan.solverExecutionTime = self.mpc.solve_time
-    longitudinalPlan.personality = self.personality
 
     pm.send('longitudinalPlan', plan_send)

selfdrive/controls/tests/test_cruise_speed.py

@@ -5,7 +5,6 @@
 
 from parameterized import parameterized_class
 from cereal import log
-from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
@@ -15,7 +14,7 @@
 ButtonType = car.CarState.ButtonEvent.Type
 
 
-def run_cruise_simulation(cruise, e2e, t_end=20.):
+def run_cruise_simulation(cruise, e2e, personality, t_end=20.):
   man = Maneuver(
     '',
     duration=t_end,
@@ -26,6 +25,7 @@ def run_cruise_simulation(cruise, e2e, t_end=20.):
     prob_lead_values=[0.0],
     breakpoints=[0.],
     e2e=e2e,
+    personality=personality,
   )
   valid, output = man.evaluate()
   assert valid
@@ -38,12 +38,10 @@ def run_cruise_simulation(cruise, e2e, t_end=20.):
                       [5,35])) # speed
 class TestCruiseSpeed(unittest.TestCase):
   def test_cruise_speed(self):
-    params = Params()
-    params.put("LongitudinalPersonality", str(self.personality))
     print(f'Testing {self.speed} m/s')
     cruise_speed = float(self.speed)
 
-    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e)
+    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality)
     self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s')
 
 

selfdrive/controls/tests/test_following_distance.py

@@ -3,14 +3,13 @@
 import itertools
 from parameterized import parameterized_class
 
-from openpilot.common.params import Params
 from cereal import log
 
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
 from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 
 
-def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
   man = Maneuver(
     '',
     duration=t_end,
@@ -20,6 +19,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
     speed_lead_values=[v_lead],
     breakpoints=[0.],
     e2e=e2e,
+    personality=personality,
   )
   valid, output = man.evaluate()
   assert valid
@@ -34,10 +34,8 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
                       [0,10,35])) # speed
 class TestFollowingDistance(unittest.TestCase):
   def test_following_distance(self):
-    params = Params()
-    params.put("LongitudinalPersonality", str(self.personality))
     v_lead = float(self.speed)
-    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
     correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
     err_ratio = 0.2 if self.e2e else 0.1
     self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))

selfdrive/test/longitudinal_maneuvers/maneuver.py

@@ -19,6 +19,7 @@ def __init__(self, title, duration, **kwargs):
     self.ensure_start = kwargs.get("ensure_start", False)
     self.enabled = kwargs.get("enabled", True)
     self.e2e = kwargs.get("e2e", False)
+    self.personality = kwargs.get("personality", 0)
     self.force_decel = kwargs.get("force_decel", False)
 
     self.duration = duration
@@ -33,6 +34,7 @@ def evaluate(self):
       only_lead2=self.only_lead2,
       only_radar=self.only_radar,
       e2e=self.e2e,
+      personality=self.personality,
       force_decel=self.force_decel,
     )
 

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -15,7 +15,7 @@ class Plant:
   messaging_initialized = False
 
   def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
-               enabled=True, only_lead2=False, only_radar=False, e2e=False, force_decel=False):
+               enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False):
     self.rate = 1. / DT_MDL
 
     if not Plant.messaging_initialized:
@@ -39,6 +39,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
     self.only_lead2 = only_lead2
     self.only_radar = only_radar
     self.e2e = e2e
+    self.personality = personality
     self.force_decel = force_decel
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
@@ -112,6 +113,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     control.controlsState.vCruise = float(v_cruise * 3.6)
     control.controlsState.experimentalMode = self.e2e
+    control.controlsState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-4f02bcfbf45697c5e6ba0a032797f6b2f37e16d3
+5eedd32ba97f9109c64059afc8e0fb827567f2ed