controlsd: pull out selfdrive state machine (#33477)

* controlsd: pull out selfdrive state machine

* cleanup test

* cleanup

selfdrive/controls/controlsd.py

@@ -29,11 +29,11 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.selfdrive.controls.lib.selfdrive import StateMachine
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
 from openpilot.system.hardware import HARDWARE
 
-SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
 CAMERA_OFFSET = 0.04
@@ -58,8 +58,6 @@ SafetyModel = car.CarParams.SafetyModel
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
-ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
-ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
 
 
 class Controls:
@@ -141,10 +139,8 @@ class Controls:
       self.LaC = LatControlTorque(self.CP, self.CI)
 
     self.initialized = False
-    self.state = State.disabled
     self.enabled = False
     self.active = False
-    self.soft_disable_timer = 0
     self.mismatch_counter = 0
     self.cruise_mismatch_counter = 0
     self.last_blinker_frame = 0
@@ -152,7 +148,6 @@ class Controls:
     self.distance_traveled = 0
     self.last_functional_fan_frame = 0
     self.events_prev = []
-    self.current_alert_types = [ET.PERMANENT]
     self.logged_comm_issue = None
     self.not_running_prev = None
     self.steer_limited = False
@@ -160,6 +155,7 @@ class Controls:
     self.experimental_mode = False
     self.personality = self.read_personality_param()
     self.recalibrating_seen = False
+    self.state_machine = StateMachine()
 
     self.can_log_mono_time = 0
 
@@ -181,7 +177,7 @@ class Controls:
 
   def set_initial_state(self):
     if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
-        self.state = State.enabled
+        self.state_machine.state = State.enabled
 
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
@@ -439,89 +435,6 @@ class Controls:
 
     return CS
 
-  def state_transition(self, CS):
-    """Compute conditional state transitions and execute actions on state transitions"""
-
-    # decrement the soft disable timer at every step, as it's reset on
-    # entrance in SOFT_DISABLING state
-    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
-
-    self.current_alert_types = [ET.PERMANENT]
-
-    # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
-    if self.state != State.disabled:
-      # user and immediate disable always have priority in a non-disabled state
-      if self.events.contains(ET.USER_DISABLE):
-        self.state = State.disabled
-        self.current_alert_types.append(ET.USER_DISABLE)
-
-      elif self.events.contains(ET.IMMEDIATE_DISABLE):
-        self.state = State.disabled
-        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
-
-      else:
-        # ENABLED
-        if self.state == State.enabled:
-          if self.events.contains(ET.SOFT_DISABLE):
-            self.state = State.softDisabling
-            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
-            self.current_alert_types.append(ET.SOFT_DISABLE)
-
-          elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
-            self.state = State.overriding
-            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
-
-        # SOFT DISABLING
-        elif self.state == State.softDisabling:
-          if not self.events.contains(ET.SOFT_DISABLE):
-            # no more soft disabling condition, so go back to ENABLED
-            self.state = State.enabled
-
-          elif self.soft_disable_timer > 0:
-            self.current_alert_types.append(ET.SOFT_DISABLE)
-
-          elif self.soft_disable_timer <= 0:
-            self.state = State.disabled
-
-        # PRE ENABLING
-        elif self.state == State.preEnabled:
-          if not self.events.contains(ET.PRE_ENABLE):
-            self.state = State.enabled
-          else:
-            self.current_alert_types.append(ET.PRE_ENABLE)
-
-        # OVERRIDING
-        elif self.state == State.overriding:
-          if self.events.contains(ET.SOFT_DISABLE):
-            self.state = State.softDisabling
-            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
-            self.current_alert_types.append(ET.SOFT_DISABLE)
-          elif not (self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL)):
-            self.state = State.enabled
-          else:
-            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
-
-    # DISABLED
-    elif self.state == State.disabled:
-      if self.events.contains(ET.ENABLE):
-        if self.events.contains(ET.NO_ENTRY):
-          self.current_alert_types.append(ET.NO_ENTRY)
-
-        else:
-          if self.events.contains(ET.PRE_ENABLE):
-            self.state = State.preEnabled
-          elif self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL):
-            self.state = State.overriding
-          else:
-            self.state = State.enabled
-          self.current_alert_types.append(ET.ENABLE)
-
-    # Check if openpilot is engaged and actuators are enabled
-    self.enabled = self.state in ENABLED_STATES
-    self.active = self.state in ACTIVE_STATES
-    if self.active:
-      self.current_alert_types.append(ET.WARNING)
-
   def state_control(self, CS):
     """Given the state, this function returns a CarControl packet"""
 
@@ -704,13 +617,14 @@ class Controls:
       self.events.add(EventName.ldw)
 
     clear_event_types = set()
-    if ET.WARNING not in self.current_alert_types:
+    if ET.WARNING not in self.state_machine.current_alert_types:
       clear_event_types.add(ET.WARNING)
     if self.enabled:
       clear_event_types.add(ET.NO_ENTRY)
 
     pers = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}[self.personality]
-    alerts = self.events.create_alerts(self.current_alert_types, [self.CP, CS, self.sm, self.is_metric, self.soft_disable_timer, pers])
+    alerts = self.events.create_alerts(self.state_machine.current_alert_types, [self.CP, CS, self.sm, self.is_metric,
+                                                                                self.state_machine.soft_disable_timer, pers])
     self.AM.add_many(self.sm.frame, alerts)
     current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types)
     if current_alert:
@@ -725,7 +639,7 @@ class Controls:
         self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
 
     force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
-                  (self.state == State.softDisabling)
+                  (self.state_machine.state == State.softDisabling)
 
     # Curvature & Steering angle
     lp = self.sm['liveParameters']
@@ -773,7 +687,7 @@ class Controls:
       ss.alertSound = current_alert.audible_alert
     ss.enabled = self.enabled
     ss.active = self.active
-    ss.state = self.state
+    ss.state = self.state_machine.state
     ss.engageable = not self.events.contains(ET.NO_ENTRY)
     ss.experimentalMode = self.experimental_mode
     ss.personality = self.personality
@@ -802,8 +716,7 @@ class Controls:
     cloudlog.timestamp("Events updated")
 
     if not self.CP.passive and self.initialized:
-      # Update control state
+      self.enabled, self.active = self.state_machine.update(self.events)
-      self.state_transition(CS)
 
     # Compute actuators (runs PID loops and lateral MPC)
     CC, lac_log = self.state_control(CS)

selfdrive/controls/lib/selfdrive.py

@@ -0,0 +1,98 @@
+from cereal import log
+from openpilot.selfdrive.controls.lib.events import Events, ET
+from openpilot.common.realtime import DT_CTRL
+
+State = log.SelfdriveState.OpenpilotState
+
+SOFT_DISABLE_TIME = 3  # seconds
+ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
+ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
+
+class StateMachine:
+  def __init__(self):
+    self.current_alert_types = [ET.PERMANENT]
+    self.state = State.disabled
+    self.soft_disable_timer = 0
+
+  def update(self, events: Events):
+    # decrement the soft disable timer at every step, as it's reset on
+    # entrance in SOFT_DISABLING state
+    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
+
+    self.current_alert_types = [ET.PERMANENT]
+
+    # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
+    if self.state != State.disabled:
+      # user and immediate disable always have priority in a non-disabled state
+      if events.contains(ET.USER_DISABLE):
+        self.state = State.disabled
+        self.current_alert_types.append(ET.USER_DISABLE)
+
+      elif events.contains(ET.IMMEDIATE_DISABLE):
+        self.state = State.disabled
+        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
+
+      else:
+        # ENABLED
+        if self.state == State.enabled:
+          if events.contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+
+          elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
+            self.state = State.overriding
+            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
+
+        # SOFT DISABLING
+        elif self.state == State.softDisabling:
+          if not events.contains(ET.SOFT_DISABLE):
+            # no more soft disabling condition, so go back to ENABLED
+            self.state = State.enabled
+
+          elif self.soft_disable_timer > 0:
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+
+          elif self.soft_disable_timer <= 0:
+            self.state = State.disabled
+
+        # PRE ENABLING
+        elif self.state == State.preEnabled:
+          if not events.contains(ET.PRE_ENABLE):
+            self.state = State.enabled
+          else:
+            self.current_alert_types.append(ET.PRE_ENABLE)
+
+        # OVERRIDING
+        elif self.state == State.overriding:
+          if events.contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+          elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)):
+            self.state = State.enabled
+          else:
+            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
+
+    # DISABLED
+    elif self.state == State.disabled:
+      if events.contains(ET.ENABLE):
+        if events.contains(ET.NO_ENTRY):
+          self.current_alert_types.append(ET.NO_ENTRY)
+
+        else:
+          if events.contains(ET.PRE_ENABLE):
+            self.state = State.preEnabled
+          elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
+            self.state = State.overriding
+          else:
+            self.state = State.enabled
+          self.current_alert_types.append(ET.ENABLE)
+
+    # Check if openpilot is engaged and actuators are enabled
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+    if active:
+      self.current_alert_types.append(ET.WARNING)
+    return enabled, active
+

selfdrive/controls/tests/test_state_machine.py

@@ -1,10 +1,7 @@
-from cereal import car, log
+from cereal import log
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.mock.values import CAR as MOCK
 from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME
+from openpilot.selfdrive.controls.lib.selfdrive import StateMachine, SOFT_DISABLE_TIME
-from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \
+from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, NormalPermanentAlert
-                                          AudibleAlert, EVENTS
 
 State = log.SelfdriveState.OpenpilotState
 
@@ -19,84 +16,77 @@ ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE
 def make_event(event_types):
   event = {}
   for ev in event_types:
-    event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW,
+    event[ev] = NormalPermanentAlert("alert")
-                      VisualAlert.none, AudibleAlert.none, 1.)
   EVENTS[0] = event
   return 0
 
 
 class TestStateMachine:
-
   def setup_method(self):
-    CarInterface, CarController, CarState, RadarInterface = interfaces[MOCK.MOCK]
+    self.events = Events()
-    CP = CarInterface.get_non_essential_params(MOCK.MOCK)
+    self.state_machine = StateMachine()
-    CI = CarInterface(CP, CarController, CarState)
+    self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
-
-    self.controlsd = Controls(CI=CI)
-    self.controlsd.events = Events()
-    self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
-    self.CS = car.CarState()
 
   def test_immediate_disable(self):
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
+        self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
-        self.controlsd.state = state
+        self.state_machine.state = state
-        self.controlsd.state_transition(self.CS)
+        self.state_machine.update(self.events)
-        assert State.disabled == self.controlsd.state
+        assert State.disabled == self.state_machine.state
-        self.controlsd.events.clear()
+        self.events.clear()
 
   def test_user_disable(self):
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.USER_DISABLE]))
+        self.events.add(make_event([et, ET.USER_DISABLE]))
-        self.controlsd.state = state
+        self.state_machine.state = state
-        self.controlsd.state_transition(self.CS)
+        self.state_machine.update(self.events)
-        assert State.disabled == self.controlsd.state
+        assert State.disabled == self.state_machine.state
-        self.controlsd.events.clear()
+        self.events.clear()
 
   def test_soft_disable(self):
     for state in ALL_STATES:
       if state == State.preEnabled:  # preEnabled considers NO_ENTRY instead
         continue
       for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE]))
+        self.events.add(make_event([et, ET.SOFT_DISABLE]))
-        self.controlsd.state = state
+        self.state_machine.state = state
-        self.controlsd.state_transition(self.CS)
+        self.state_machine.update(self.events)
-        assert self.controlsd.state == State.disabled if state == State.disabled else State.softDisabling
+        assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
-        self.controlsd.events.clear()
+        self.events.clear()
 
   def test_soft_disable_timer(self):
-    self.controlsd.state = State.enabled
+    self.state_machine.state = State.enabled
-    self.controlsd.events.add(make_event([ET.SOFT_DISABLE]))
+    self.events.add(make_event([ET.SOFT_DISABLE]))
-    self.controlsd.state_transition(self.CS)
+    self.state_machine.update(self.events)
     for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
-      assert self.controlsd.state == State.softDisabling
+      assert self.state_machine.state == State.softDisabling
-      self.controlsd.state_transition(self.CS)
+      self.state_machine.update(self.events)
 
-    assert self.controlsd.state == State.disabled
+    assert self.state_machine.state == State.disabled
 
   def test_no_entry(self):
     # Make sure noEntry keeps us disabled
     for et in ENABLE_EVENT_TYPES:
-      self.controlsd.events.add(make_event([ET.NO_ENTRY, et]))
+      self.events.add(make_event([ET.NO_ENTRY, et]))
-      self.controlsd.state_transition(self.CS)
+      self.state_machine.update(self.events)
-      assert self.controlsd.state == State.disabled
+      assert self.state_machine.state == State.disabled
-      self.controlsd.events.clear()
+      self.events.clear()
 
   def test_no_entry_pre_enable(self):
     # preEnabled with noEntry event
-    self.controlsd.state = State.preEnabled
+    self.state_machine.state = State.preEnabled
-    self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
+    self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
-    self.controlsd.state_transition(self.CS)
+    self.state_machine.update(self.events)
-    assert self.controlsd.state == State.preEnabled
+    assert self.state_machine.state == State.preEnabled
 
   def test_maintain_states(self):
     # Given current state's event type, we should maintain state
     for state in ALL_STATES:
       for et in MAINTAIN_STATES[state]:
-        self.controlsd.state = state
+        self.state_machine.state = state
-        self.controlsd.events.add(make_event([et]))
+        self.events.add(make_event([et]))
-        self.controlsd.state_transition(self.CS)
+        self.state_machine.update(self.events)
-        assert self.controlsd.state == state
+        assert self.state_machine.state == state
-        self.controlsd.events.clear()
+        self.events.clear()