mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 21:14:01 +08:00
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
This commit is contained in:
Adeeb Shihadeh
2024-09-04 16:44:28 -07:00
committed by GitHub
parent 54a2626e44
commit 77f4f57e73
3 changed files with 148 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
selfdrive/controls/controlsd.py
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 Normal file
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
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()