mirror of https://github.com/commaai/openpilot.git
controlsd: split up publishing and logic (#33483)
* split up the pubs * move all event adds * split out alerts * lil more * clenaup * update test * cleanup
This commit is contained in:
Adeeb Shihadeh
GitHub
parent
e0be4a1f85
commit
aef650013e
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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=}"
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
c2022c388da6eb2e26bb23ad6009be9d5314c0be
|
||||
fb0827a00b21b97733f8385cf7ce87a21a526f4c
|
||||
Loading…
Reference in New Issue