sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

controlsd: pull out selfdrive state machine (#33477)

Browse Source 
* controlsd: pull out selfdrive state machine

* cleanup test

* cleanup
pull/33479/head
Adeeb Shihadeh 8 months ago committed by GitHub
parent 54a2626e44
commit 77f4f57e73
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
3 changed files with 148 additions and 147 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 105
      selfdrive/controls/controlsd.py
  2. 98
      selfdrive/controls/lib/selfdrive.py
  3. 92
      selfdrive/controls/tests/test_state_machine.py

105
selfdrive/controls/controlsd.py
Unescape View File

@ -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.state_transition(CS)
self.enabled, self.active = self.state_machine.update(self.events)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)

98
selfdrive/controls/lib/selfdrive.py
Unescape View File

@ -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

92
selfdrive/controls/tests/test_state_machine.py
Unescape View File

@ -1,10 +1,7 @@
from cereal import car, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.mock.values import CAR as MOCK
from cereal import log
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME
from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \
AudibleAlert, EVENTS
from openpilot.selfdrive.controls.lib.selfdrive import StateMachine, SOFT_DISABLE_TIME
from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, NormalPermanentAlert
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,
VisualAlert.none, AudibleAlert.none, 1.)
event[ev] = NormalPermanentAlert("alert")
EVENTS[0] = event
return 0
class TestStateMachine:
def setup_method(self):
CarInterface, CarController, CarState, RadarInterface = interfaces[MOCK.MOCK]
CP = CarInterface.get_non_essential_params(MOCK.MOCK)
CI = CarInterface(CP, CarController, CarState)
self.controlsd = Controls(CI=CI)
self.controlsd.events = Events()
self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
self.CS = car.CarState()
self.events = Events()
self.state_machine = StateMachine()
self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
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.controlsd.state = state
self.controlsd.state_transition(self.CS)
assert State.disabled == self.controlsd.state
self.controlsd.events.clear()
self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert State.disabled == self.state_machine.state
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.controlsd.state = state
self.controlsd.state_transition(self.CS)
assert State.disabled == self.controlsd.state
self.controlsd.events.clear()
self.events.add(make_event([et, ET.USER_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert State.disabled == self.state_machine.state
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.controlsd.state = state
self.controlsd.state_transition(self.CS)
assert self.controlsd.state == State.disabled if state == State.disabled else State.softDisabling
self.controlsd.events.clear()
self.events.add(make_event([et, ET.SOFT_DISABLE]))
self.state_machine.state = state
self.state_machine.update(self.events)
assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
self.events.clear()
def test_soft_disable_timer(self):
self.controlsd.state = State.enabled
self.controlsd.events.add(make_event([ET.SOFT_DISABLE]))
self.controlsd.state_transition(self.CS)
self.state_machine.state = State.enabled
self.events.add(make_event([ET.SOFT_DISABLE]))
self.state_machine.update(self.events)
for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
assert self.controlsd.state == State.softDisabling
self.controlsd.state_transition(self.CS)
assert self.state_machine.state == State.softDisabling
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.controlsd.state_transition(self.CS)
assert self.controlsd.state == State.disabled
self.controlsd.events.clear()
self.events.add(make_event([ET.NO_ENTRY, et]))
self.state_machine.update(self.events)
assert self.state_machine.state == State.disabled
self.events.clear()
def test_no_entry_pre_enable(self):
# preEnabled with noEntry event
self.controlsd.state = State.preEnabled
self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
self.controlsd.state_transition(self.CS)
assert self.controlsd.state == State.preEnabled
self.state_machine.state = State.preEnabled
self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
self.state_machine.update(self.events)
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.controlsd.events.add(make_event([et]))
self.controlsd.state_transition(self.CS)
assert self.controlsd.state == state
self.controlsd.events.clear()
self.state_machine.state = state
self.events.add(make_event([et]))
self.state_machine.update(self.events)
assert self.state_machine.state == state
self.events.clear()

Write Preview
Loading…
Cancel
Save