Deprecate controlsState state fields (#33437)

adeebshihadeh · web-flow · commit bd2b09c7b6bd · 2024-09-03T14:40:23.000-07:00
* Deprecate controlsState state fields * sim works * update refs * one more * these too * update sim old-commit-hash: 3924ac5
diff --git a/cereal/log.capnp b/cereal/log.capnp
@@ -739,21 +739,7 @@ struct ControlsState @0x97ff69c53601abf1 {
   aTarget @35 :Float32;
   curvature @37 :Float32;  # path curvature from vehicle model
   desiredCurvature @61 :Float32;  # lag adjusted curvatures used by lateral controllers
-
-  # TODO: remove these, they're now in selfdriveState
-  alertText1 @24 :Text;
-  alertText2 @25 :Text;
-  alertStatus @38 :SelfdriveState.AlertStatus;
-  alertSize @39 :SelfdriveState.AlertSize;
-  alertType @44 :Text;
-  alertSound @56 :Car.CarControl.HUDControl.AudibleAlert;
-  engageable @41 :Bool;  # can OP be engaged?
   forceDecel @51 :Bool;
-  state @31 :SelfdriveState.OpenpilotState;
-  enabled @19 :Bool;
-  active @36 :Bool;
-  experimentalMode @64 :Bool;
-  personality @66 :LongitudinalPersonality;
 
   lateralControlState :union {
     indiState @52 :LateralINDIState;
@@ -880,6 +866,18 @@ struct ControlsState @0x97ff69c53601abf1 {
   canErrorCounterDEPRECATED @57 :UInt32;
   vPidDEPRECATED @2 :Float32;
   alertBlinkingRateDEPRECATED @42 :Float32;
+  alertText1DEPRECATED @24 :Text;
+  alertText2DEPRECATED @25 :Text;
+  alertStatusDEPRECATED @38 :SelfdriveState.AlertStatus;
+  alertSizeDEPRECATED @39 :SelfdriveState.AlertSize;
+  alertTypeDEPRECATED @44 :Text;
+  alertSound2DEPRECATED @56 :Car.CarControl.HUDControl.AudibleAlert;
+  engageableDEPRECATED @41 :Bool;  # can OP be engaged?
+  stateDEPRECATED @31 :SelfdriveState.OpenpilotState;
+  enabledDEPRECATED @19 :Bool;
+  activeDEPRECATED @36 :Bool;
+  experimentalModeDEPRECATED @64 :Bool;
+  personalityDEPRECATED @66 :LongitudinalPersonality;
   vCruiseDEPRECATED @22 :Float32;  # actual set speed
   vCruiseClusterDEPRECATED @63 :Float32;  # set speed to display in the UI
 }
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
@@ -733,31 +733,17 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     dat = messaging.new_message('controlsState')
     dat.valid = CS.canValid
     controlsState = dat.controlsState
-    if current_alert:
-      controlsState.alertText1 = current_alert.alert_text_1
-      controlsState.alertText2 = current_alert.alert_text_2
-      controlsState.alertSize = current_alert.alert_size
-      controlsState.alertStatus = current_alert.alert_status
-      controlsState.alertType = current_alert.alert_type
-      controlsState.alertSound = current_alert.audible_alert
-
     controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
     controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
-    controlsState.enabled = self.enabled
-    controlsState.active = self.active
     controlsState.curvature = curvature
     controlsState.desiredCurvature = self.desired_curvature
-    controlsState.state = self.state
-    controlsState.engageable = not self.events.contains(ET.NO_ENTRY)
     controlsState.longControlState = self.LoC.long_control_state
     controlsState.upAccelCmd = float(self.LoC.pid.p)
     controlsState.uiAccelCmd = float(self.LoC.pid.i)
     controlsState.ufAccelCmd = float(self.LoC.pid.f)
     controlsState.cumLagMs = -self.rk.remaining * 1000.
     controlsState.startMonoTime = int(start_time * 1e9)
     controlsState.forceDecel = bool(force_decel)
-    controlsState.experimentalMode = self.experimental_mode
-    controlsState.personality = self.personality
 
     lat_tuning = self.CP.lateralTuning.which()
     if self.joystick_mode:
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
@@ -96,7 +96,7 @@ def parse_model(model_msg, model_error):
     return x, v, a, j
 
   def update(self, sm):
-    self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
+    self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
@@ -106,7 +106,7 @@ def update(self, sm):
     force_slow_decel = sm['controlsState'].forceDecel
 
     # Reset current state when not engaged, or user is controlling the speed
-    reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled
+    reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
 
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
@@ -134,11 +134,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=sm['controlsState'].personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].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=sm['controlsState'].personality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -157,7 +157,7 @@ def update(self, sm):
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')
 
-    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
 
     longitudinalPlan = plan_send.longitudinalPlan
     longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
@@ -17,7 +17,7 @@ def plannerd_thread():
 
   longitudinal_planner = LongitudinalPlanner(CP)
   pm = messaging.PubMaster(['longitudinalPlan'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 
   while True:
diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py
@@ -13,7 +13,7 @@ def dmonitoringd_thread():
 
   params = Params()
   pm = messaging.PubMaster(['driverMonitoringState'])
-  sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll='driverStateV2')
+  sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
 
   DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
 
diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py
@@ -403,13 +403,13 @@ def run_step(self, sm):
       driver_state=sm['driverStateV2'],
       cal_rpy=sm['liveCalibration'].rpyCalib,
       car_speed=sm['carState'].vEgo,
-      op_engaged=sm['controlsState'].enabled
+      op_engaged=sm['selfdriveState'].enabled
     )
 
     # Update distraction events
     self._update_events(
       driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed,
-      op_engaged=sm['controlsState'].enabled,
+      op_engaged=sm['selfdriveState'].enabled,
       standstill=sm['carState'].standstill,
       wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park],
       car_speed=sm['carState'].vEgo
diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc
@@ -313,7 +313,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
 }
 
 void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started) {
-  static SubMaster sm({"controlsState"});
+  static SubMaster sm({"selfdriveState"});
 
   std::vector<std::string> connected_serials;
   for (Panda *p : pandas) {
@@ -351,7 +351,7 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoof
     }
 
     sm.update(0);
-    const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
+    const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
     for (const auto &panda : pandas) {
       panda->send_heartbeat(engaged);
     }
diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py
@@ -21,6 +21,7 @@ def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
       Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
       Plant.car_state = messaging.pub_sock('carState')
       Plant.plan = messaging.sub_sock('longitudinalPlan')
       Plant.messaging_initialized = True
@@ -61,6 +62,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     # note that this is worst case for MPC, since model will delay long mpc by one time step
     radar = messaging.new_message('radarState')
     control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
     car_state = messaging.new_message('carState')
     model = messaging.new_message('modelV2')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
@@ -111,8 +113,8 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     model.modelV2.acceleration = acceleration
 
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
-    control.controlsState.experimentalMode = self.e2e
-    control.controlsState.personality = self.personality
+    ss.selfdriveState.experimentalMode = self.e2e
+    ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = self.speed < 0.01
@@ -122,6 +124,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.):
     sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
           'controlsState': control.controlsState,
+          'selfdriveState': ss.selfdriveState,
           'modelV2': model.modelV2}
     self.planner.update(sm)
     self.speed = self.planner.v_desired_filter.x
diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py
@@ -39,9 +39,9 @@ def migrate_controlsState(lr):
       m.logMonoTime = msg.logMonoTime
       ss = m.selfdriveState
       for field in ("enabled", "active", "state", "engageable", "alertText1", "alertText2",
-                    "alertStatus", "alertSize", "alertType", "alertSound", "experimentalMode",
+                    "alertStatus", "alertSize", "alertType", "experimentalMode",
                     "personality"):
-        setattr(ss, field, getattr(msg.controlsState, field))
+        setattr(ss, field, getattr(msg.controlsState, field+"DEPRECATED"))
       ret.append(m.as_reader())
     elif msg.which() == 'carState' and last_cs is not None:
       if last_cs.controlsState.vCruiseDEPRECATED - msg.carState.vCruise > 0.1:
diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py
@@ -506,7 +506,7 @@ def locationd_config_pubsub_callback(params, cfg, lr):
   ),
   ProcessConfig(
     proc_name="plannerd",
-    pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"],
+    pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"],
     subs=["longitudinalPlan"],
     ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
     init_callback=get_car_params_callback,
@@ -522,7 +522,7 @@ def locationd_config_pubsub_callback(params, cfg, lr):
   ),
   ProcessConfig(
     proc_name="dmonitoringd",
-    pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"],
+    pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "selfdriveState"],
     subs=["driverMonitoringState"],
     ignore=["logMonoTime"],
     should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"),
@@ -810,8 +810,8 @@ def check_openpilot_enabled(msgs: LogIterable) -> bool:
     if msg.which() == "carParams":
       if msg.carParams.notCar:
         return True
-    elif msg.which() == "controlsState":
-      if msg.controlsState.active:
+    elif msg.which() == "selfdriveState":
+      if msg.selfdriveState.active:
         cur_enabled_count += 1
       else:
         cur_enabled_count = 0
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-073dcca6932c5c66cdadf9aee9b531b623795888
+ca8cca8eeca938c3802109d6ea25cb719dcc649a
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
@@ -429,6 +429,6 @@ def test_engagable(self):
         if evt.noEntry:
           no_entries[evt.name] += 1
 
-    eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']]
+    eng = [m.selfdriveState.engageable for m in self.service_msgs['selfdriveState']]
     assert all(eng), \
-           f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}"
+           f"Not engageable for whole segment:\n- selfdriveState.engageable: {Counter(eng)}\n- No entry events: {no_entries}"
diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py
@@ -20,7 +20,7 @@ def test_time_to_onroad():
   proc = subprocess.Popen(["python", manager_path])
 
   start_time = time.monotonic()
-  sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents'])
+  sm = messaging.SubMaster(['selfdriveState', 'controlsState', 'deviceState', 'onroadEvents'])
   try:
     # wait for onroad. timeout assumes panda is up to date
     with Timeout(10, "timed out waiting to go onroad"):
@@ -39,7 +39,7 @@ def test_time_to_onroad():
 
           if initialized:
             sm.update(100)
-            assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}"
+            assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
             break
     finally:
       print(f"onroad events: {sm['onroadEvents']}")
@@ -50,7 +50,7 @@ def test_time_to_onroad():
     while (time.monotonic() - st) < 10.:
       sm.update(100)
       assert sm.all_alive(), sm.alive
-      assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}"
+      assert sm['selfdriveState'].engageable, f"events: {sm['onroadEvents']}"
       assert sm['controlsState'].cumLagMs < 10.
   finally:
     proc.terminate()
diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py
@@ -164,7 +164,7 @@ def hw_state_thread(end_event, hw_queue):
 
 def hardware_thread(end_event, hw_queue) -> None:
   pm = messaging.PubMaster(['deviceState'])
-  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll="pandaStates")
+  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates")
 
   count = 0
 
@@ -341,8 +341,8 @@ def hardware_thread(end_event, hw_queue) -> None:
       engaged_prev = False
       HARDWARE.set_power_save(not should_start)
 
-    if sm.updated['controlsState']:
-      engaged = sm['controlsState'].enabled
+    if sm.updated['selfdriveState']:
+      engaged = sm['selfdriveState'].enabled
       if engaged != engaged_prev:
         params.put_bool("IsEngaged", engaged)
         engaged_prev = engaged
diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc
@@ -90,18 +90,18 @@ void LogReader::migrateOldEvents() {
       new_evt.setLogMonoTime(old_evt.getLogMonoTime());
       auto new_state = new_evt.initSelfdriveState();
 
-      new_state.setActive(old_state.getActive());
-      new_state.setAlertSize(old_state.getAlertSize());
-      new_state.setAlertSound(old_state.getAlertSound());
-      new_state.setAlertStatus(old_state.getAlertStatus());
-      new_state.setAlertText1(old_state.getAlertText1());
-      new_state.setAlertText2(old_state.getAlertText2());
-      new_state.setAlertType(old_state.getAlertType());
-      new_state.setEnabled(old_state.getEnabled());
-      new_state.setEngageable(old_state.getEngageable());
-      new_state.setExperimentalMode(old_state.getExperimentalMode());
-      new_state.setPersonality(old_state.getPersonality());
-      new_state.setState(old_state.getState());
+      new_state.setActive(old_state.getActiveDEPRECATED());
+      new_state.setAlertSize(old_state.getAlertSizeDEPRECATED());
+      new_state.setAlertSound(old_state.getAlertSound2DEPRECATED());
+      new_state.setAlertStatus(old_state.getAlertStatusDEPRECATED());
+      new_state.setAlertText1(old_state.getAlertText1DEPRECATED());
+      new_state.setAlertText2(old_state.getAlertText2DEPRECATED());
+      new_state.setAlertType(old_state.getAlertTypeDEPRECATED());
+      new_state.setEnabled(old_state.getEnabledDEPRECATED());
+      new_state.setEngageable(old_state.getEngageableDEPRECATED());
+      new_state.setExperimentalMode(old_state.getExperimentalModeDEPRECATED());
+      new_state.setPersonality(old_state.getPersonalityDEPRECATED());
+      new_state.setState(old_state.getStateDEPRECATED());
 
       // Serialize the new event to the buffer
       auto buf_size = msg.getSerializedSize();
diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py
@@ -167,16 +167,15 @@ def _run(self, q: Queue):
       self.simulated_sensors.update(self.simulator_state, self.world)
 
       self.simulated_car.sm.update(0)
-      controlsState = self.simulated_car.sm['controlsState']
-      self.simulator_state.is_engaged = controlsState.active
+      self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
 
       if self.simulator_state.is_engaged:
         throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
         brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
         steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
 
         self.past_startup_engaged = True
-      elif not self.past_startup_engaged and controlsState.engageable:
+      elif not self.past_startup_engaged and self.simulated_car.sm['selfdriveState'].engageable:
         self.simulator_state.cruise_button = CruiseButtons.DECEL_SET if self.startup_button_prev else CruiseButtons.MAIN # force engagement on startup
         self.startup_button_prev = not self.startup_button_prev
 
diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py
@@ -14,7 +14,7 @@ class SimulatedCar:
 
   def __init__(self):
     self.pm = messaging.PubMaster(['can', 'pandaStates'])
-    self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams'])
+    self.sm = messaging.SubMaster(['carControl', 'controlsState', 'carParams', 'selfdriveState'])
     self.cp = self.get_car_can_parser()
     self.idx = 0
     self.params = Params()
diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py
@@ -25,7 +25,7 @@ def test_driving(self):
     p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR)
     self.processes.append(p_manager)
 
-    sm = messaging.SubMaster(['controlsState', 'onroadEvents', 'managerState'])
+    sm = messaging.SubMaster(['selfdriveState', 'onroadEvents', 'managerState'])
     q = Queue()
     bridge = self.create_bridge()
     p_bridge = bridge.run(q, retries=10)
@@ -63,7 +63,7 @@ def test_driving(self):
     while time.monotonic() < start_time + max_time_per_step:
       sm.update()
 
-      if sm.all_alive() and sm['controlsState'].active:
+      if sm.all_alive() and sm['selfdriveState'].active:
         control_active += 1
 
         if control_active == min_counts_control_active:

Original file line number	Diff line number	Diff line change
`@@ -313,7 +313,7 @@ void send_peripheral_state(Panda panda, PubMaster pm) {`
`313`	`313`	`}`
`314`	`314`
`315`	`315`	`void process_panda_state(std::vector<Panda > &pandas, PubMaster pm, bool spoofing_started) {`
`316`		`- static SubMaster sm({"controlsState"});`
	`316`	`+ static SubMaster sm({"selfdriveState"});`
`317`	`317`
`318`	`318`	`std::vector<std::string> connected_serials;`
`319`	`319`	`for (Panda *p : pandas) {`
`@@ -351,7 +351,7 @@ void process_panda_state(std::vector<Panda > &pandas, PubMaster pm, bool spoof`
`351`	`351`	`}`
`352`	`352`
`353`	`353`	`sm.update(0);`
`354`		`- const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();`
	`354`	`+ const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();`
`355`	`355`	`for (const auto &panda : pandas) {`
`356`	`356`	`panda->send_heartbeat(engaged);`
`357`	`357`	`}`