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

controlsd: split up publishing and logic (#33483)

Browse Source 
* split up the pubs

* move all event adds

* split out alerts

* lil more

* clenaup

* update test

* cleanup
pull/33443/head
Adeeb Shihadeh 8 months ago committed by GitHub
parent e0be4a1f85
commit aef650013e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 106 additions and 107 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. 192
      selfdrive/controls/controlsd.py
  2. 11
      selfdrive/controls/lib/alertmanager.py
  3. 4
      selfdrive/controls/lib/tests/test_alertmanager.py
  4. 4
      selfdrive/debug/count_events.py
  5. 2
      selfdrive/test/process_replay/ref_commit

192
selfdrive/controls/controlsd.py
Unescape View File

@ -117,6 +117,8 @@ class Controls:
self.params.remove("ExperimentalMode")
self.CS_prev = car.CarState.new_message()
self.CC_prev = car.CarControl.new_message()
self.lac_log_prev = None
self.AM = AlertManager()
self.events = Events()
@ -216,16 +218,10 @@ class Controls:
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
if self.sm['deviceState'].memoryUsagePercent > 90 and not SIMULATION:
self.events.add(EventName.lowMemory)
# TODO: enable this once loggerd CPU usage is more reasonable
#cpus = list(self.sm['deviceState'].cpuUsagePercent)
#if max(cpus, default=0) > 95 and not SIMULATION:
# self.events.add(EventName.highCpuUsage)
# Alert if fan isn't spinning for 5 seconds
if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
@ -248,6 +244,13 @@ class Controls:
else:
self.events.add(EventName.calibrationInvalid)
# Lane departure warning
if self.sm.valid['modelV2'] and CS.canValid:
self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, self.CC_prev)
if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
if self.ldw.warning:
self.events.add(EventName.ldw)
# Handle lane change
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
@ -348,6 +351,37 @@ class Controls:
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# Send a "steering required alert" if saturation count has reached the limit
lac_log = self.lac_log_prev
if CS.steeringPressed:
self.last_steering_pressed_frame = self.sm.frame
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
if self.lac_log_prev is not None and self.lac_log_prev.active and not recent_steer_pressed and not self.CP.notCar:
lac_log = self.lac_log_prev
if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
good_speed = CS.vEgo > 5
max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
self.events.add(EventName.steerSaturated)
elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature
dpath_points = self.sm['modelV2'].position.y
if len(dpath_points):
# Check if we deviated from the path
# TODO use desired vs actual curvature
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
steering_value = self.CC_prev.actuators.steeringAngleDeg
else:
steering_value = self.CC_prev.actuators.steer
left_deviation = steering_value > 0 and dpath_points[0] < -0.20
right_deviation = steering_value < 0 and dpath_points[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Check for FCW
stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
@ -521,36 +555,6 @@ class Controls:
lac_log.output = actuators.steer
lac_log.saturated = abs(actuators.steer) >= 0.9
if CS.steeringPressed:
self.last_steering_pressed_frame = self.sm.frame
recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.active and not recent_steer_pressed and not self.CP.notCar:
if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
turning = abs(lac_log.desiredLateralAccel) > 1.0
good_speed = CS.vEgo > 5
max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99
if undershooting and turning and good_speed and max_torque:
lac_log.active and self.events.add(EventName.steerSaturated)
elif lac_log.saturated:
# TODO probably should not use dpath_points but curvature
dpath_points = model_v2.position.y
if len(dpath_points):
# Check if we deviated from the path
# TODO use desired vs actual curvature
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
steering_value = actuators.steeringAngleDeg
else:
steering_value = actuators.steer
left_deviation = steering_value > 0 and dpath_points[0] < -0.20
right_deviation = steering_value < 0 and dpath_points[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
@ -563,9 +567,20 @@ class Controls:
return CC, lac_log
def publish_logs(self, CS, CC, lac_log):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
def update_alerts(self, CS):
clear_event_types = set()
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.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)
self.AM.process_alerts(self.sm.frame, clear_event_types)
def publish_carControl(self, CS, CC, lac_log):
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller
if self.calibrated_pose is not None:
@ -591,26 +606,12 @@ class Controls:
hudControl.rightLaneVisible = True
hudControl.leftLaneVisible = True
self.ldw.update(self.sm.frame, self.sm['modelV2'], CS, CC)
if self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
if self.sm.valid['modelV2'] and CS.canValid and self.is_ldw_enabled and self.sm['liveCalibration'].calStatus == log.LiveCalibrationData.Status.calibrated:
hudControl.leftLaneDepart = self.ldw.left
hudControl.rightLaneDepart = self.ldw.right
if self.ldw.warning:
self.events.add(EventName.ldw)
clear_event_types = set()
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.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:
hudControl.visualAlert = current_alert.visual_alert
if self.AM.current_alert:
hudControl.visualAlert = self.AM.current_alert.visual_alert
if not self.CP.passive and self.initialized:
CO = self.sm['carOutput']
@ -620,53 +621,57 @@ class Controls:
else:
self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state_machine.state == State.softDisabling)
# Curvature & Steering angle
lp = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
# controlsState
dat = messaging.new_message('controlsState')
dat.valid = CS.canValid
controlsState = dat.controlsState
controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
controlsState.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
controlsState.curvature = curvature
controlsState.desiredCurvature = self.desired_curvature
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.forceDecel = bool(force_decel)
cs = dat.controlsState
lp = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
cs.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
cs.desiredCurvature = self.desired_curvature
cs.longControlState = self.LoC.long_control_state
cs.upAccelCmd = float(self.LoC.pid.p)
cs.uiAccelCmd = float(self.LoC.pid.i)
cs.ufAccelCmd = float(self.LoC.pid.f)
cs.cumLagMs = -self.rk.remaining * 1000.
cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
(self.state_machine.state == State.softDisabling))
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
controlsState.lateralControlState.debugState = lac_log
cs.lateralControlState.debugState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.angle:
controlsState.lateralControlState.angleState = lac_log
cs.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
cs.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
controlsState.lateralControlState.torqueState = lac_log
cs.lateralControlState.torqueState = lac_log
self.pm.send('controlsState', dat)
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
def publish_selfdriveState(self, CS):
# selfdriveState
ss_msg = messaging.new_message('selfdriveState')
ss_msg.valid = CS.canValid
ss = ss_msg.selfdriveState
if current_alert:
ss.alertText1 = current_alert.alert_text_1
ss.alertText2 = current_alert.alert_text_2
ss.alertSize = current_alert.alert_size
ss.alertStatus = current_alert.alert_status
ss.alertType = current_alert.alert_type
ss.alertSound = current_alert.audible_alert
if self.AM.current_alert:
ss.alertText1 = self.AM.current_alert.alert_text_1
ss.alertText2 = self.AM.current_alert.alert_text_2
ss.alertSize = self.AM.current_alert.alert_size
ss.alertStatus = self.AM.current_alert.alert_status
ss.alertType = self.AM.current_alert.alert_type
ss.alertSound = self.AM.current_alert.audible_alert
ss.enabled = self.enabled
ss.active = self.active
ss.state = self.state_machine.state
@ -683,30 +688,23 @@ class Controls:
self.pm.send('onroadEvents', ce_send)
self.events_prev = self.events.names.copy()
# carControl
cc_send = messaging.new_message('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
def step(self):
# Sample data from sockets and get a carState
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
self.update_events(CS)
cloudlog.timestamp("Events updated")
if not self.CP.passive and self.initialized:
self.enabled, self.active = self.state_machine.update(self.events)
self.update_alerts(CS)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
# Publish data
self.publish_logs(CS, CC, lac_log)
self.publish_carControl(CS, CC, lac_log)
self.publish_selfdriveState(CS)
self.CS_prev = CS
self.CC_prev = CC
self.lac_log_prev = lac_log
def read_personality_param(self):
try:

11
selfdrive/controls/lib/alertmanager.py
Unescape View File

@ -34,6 +34,7 @@ class AlertEntry:
class AlertManager:
def __init__(self):
self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
self.current_alert: Alert | None = None
def add_many(self, frame: int, alerts: list[Alert]) -> None:
for alert in alerts:
@ -44,8 +45,8 @@ class AlertManager:
min_end_frame = entry.start_frame + alert.duration
entry.end_frame = max(frame + 1, min_end_frame)
def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None:
current_alert = AlertEntry()
def process_alerts(self, frame: int, clear_event_types: set):
ae = AlertEntry()
for v in self.alerts.values():
if not v.alert:
continue
@ -54,8 +55,8 @@ class AlertManager:
v.end_frame = -1
# sort by priority first and then by start_frame
greater = current_alert.alert is None or (v.alert.priority, v.start_frame) > (current_alert.alert.priority, current_alert.start_frame)
greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
if v.active(frame) and greater:
current_alert = v
ae = v
return current_alert.alert
self.current_alert = ae.alert

4
selfdrive/controls/lib/tests/test_alertmanager.py
Unescape View File

@ -32,8 +32,8 @@ class TestAlertManager:
for frame in range(duration+10):
if frame < add_duration:
AM.add_many(frame, [alert, ])
current_alert = AM.process_alerts(frame, {})
AM.process_alerts(frame, {})
shown = current_alert is not None
shown = AM.current_alert is not None
should_show = frame <= show_duration
assert shown == should_show, f"{frame=} {add_duration=} {duration=}"

4
selfdrive/debug/count_events.py
Unescape View File

@ -33,8 +33,8 @@ if __name__ == "__main__":
if len(events) == 0 or ae != events[-1][1]:
events.append((t, ae))
elif msg.which() == 'controlsState':
at = msg.controlsState.alertType
elif msg.which() == 'selfdriveState':
at = msg.selfdriveState.alertType
if "/override" not in at or "lanechange" in at.lower():
if len(alerts) == 0 or alerts[-1][1] != at:
alerts.append((t, at))

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
c2022c388da6eb2e26bb23ad6009be9d5314c0be
fb0827a00b21b97733f8385cf7ce87a21a526f4c
Write Preview
Loading…
Cancel
Save