Setup driverAssistance service (#33495)

* Move LDW and FCW to driverAssitance service

* move ldw

* cleanup

cereal/log.capnp

@@ -1096,6 +1096,14 @@ struct AndroidLogEntry {
   message @6 :Text;
 }
 
+struct DriverAssistance {
+  # Lane Departure Warnings
+  leftLaneDeparture @0 :Bool;
+  rightLaneDeparture @1 :Bool;
+
+  # FCW, AEB, etc. will go here
+}
+
 struct LongitudinalPlan @0xe00b5b3eba12876c {
   modelMonoTime @9 :UInt64;
   hasLead @7 :Bool;
@@ -2342,6 +2350,7 @@ struct Event {
     carControl @23 :Car.CarControl;
     carOutput @127 :Car.CarOutput;
     longitudinalPlan @24 :LongitudinalPlan;
+    driverAssistance @132 :DriverAssistance;
     ubloxGnss @34 :UbloxGnss;
     ubloxRaw @39 :Data;
     qcomGnss @31 :QcomGnss;

cereal/services.py

@@ -40,6 +40,7 @@ _services: dict[str, tuple] = {
   "carControl": (True, 100., 10),
   "carOutput": (True, 100., 10),
   "longitudinalPlan": (True, 20., 10),
+  "driverAssistance": (True, 20., 20),
   "procLog": (True, 0.5, 15),
   "gpsLocationExternal": (True, 10., 10),
   "gpsLocation": (True, 1., 1),

selfdrive/controls/controlsd.py

@@ -22,7 +22,6 @@ from openpilot.common.gps import get_gps_location_service
 from opendbc.car.car_helpers import get_car_interface
 from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_startup_event
-from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
 from openpilot.selfdrive.controls.lib.events import Events, ET
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -95,7 +94,7 @@ class Controls:
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'testJoystick'] + self.camera_packets + self.sensor_packets + self.gps_packets,
+                                   'testJoystick', 'driverAssistance'] + self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore+['testJoystick'], ignore_valid=['testJoystick', ],
                                   frequency=int(1/DT_CTRL))
 
@@ -128,8 +127,6 @@ class Controls:
     self.LoC = LongControl(self.CP)
     self.VM = VehicleModel(self.CP)
 
-    self.ldw = LaneDepartureWarning()
-
     self.LaC: LatControl
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP, self.CI)
@@ -245,11 +242,9 @@ class Controls:
         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)
+    if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
+      if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
+        self.events.add(EventName.ldw)
 
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
@@ -606,9 +601,9 @@ class Controls:
     hudControl.rightLaneVisible = True
     hudControl.leftLaneVisible = True
 
-    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.is_ldw_enabled and self.sm.valid['driverAssistance']:
+      hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
+      hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
 
     if self.AM.current_alert:
       hudControl.visualAlert = self.AM.current_alert.visual_alert

selfdrive/controls/plannerd.py

@@ -3,11 +3,12 @@ from cereal import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
 import cereal.messaging as messaging
 
 
-def plannerd_thread():
+def main():
   config_realtime_process(5, Priority.CTRL_LOW)
 
   cloudlog.info("plannerd is waiting for CarParams")
@@ -15,8 +16,9 @@ def plannerd_thread():
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("plannerd got CarParams: %s", CP.carName)
 
+  ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP)
-  pm = messaging.PubMaster(['longitudinalPlan'])
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 
@@ -26,9 +28,12 @@ def plannerd_thread():
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
 
-
-def main():
-  plannerd_thread()
+      ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
+      msg = messaging.new_message('driverAssistance')
+      msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2'])
+      msg.driverAssistance.leftLaneDeparture = ldw.left
+      msg.driverAssistance.rightLaneDeparture = ldw.right
+      pm.send('driverAssistance', msg)
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/migration.py

@@ -18,6 +18,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
   msgs = migrate_controlsState(msgs)
   msgs = migrate_liveLocationKalman(msgs)
   msgs = migrate_liveTracks(msgs)
+  msgs = migrate_driverAssistance(msgs)
   if manager_states:
     msgs = migrate_managerState(msgs)
   if panda_states:
@@ -28,6 +29,15 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
 
   return msgs
 
+def migrate_driverAssistance(lr):
+  all_msgs = []
+  for msg in lr:
+    all_msgs.append(msg)
+    if msg.which() == 'longitudinalPlan':
+      all_msgs.append(messaging.new_message('driverAssistance', valid=True, logMonoTime=msg.logMonoTime).as_reader())
+    if msg.which() == 'driverAssistance':
+      return lr
+  return all_msgs
 
 def migrate_liveTracks(lr):
   all_msgs = []

selfdrive/test/process_replay/process_replay.py

@@ -467,7 +467,7 @@ CONFIGS = [
       "longitudinalPlan", "livePose", "liveParameters", "radarState",
       "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState",
       "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput",
-      "gpsLocationExternal", "gpsLocation",
+      "gpsLocationExternal", "gpsLocation", "driverAssistance"
     ],
     subs=["selfdriveState", "controlsState", "carControl", "onroadEvents"],
     ignore=["logMonoTime", "controlsState.cumLagMs"],
@@ -499,7 +499,7 @@ CONFIGS = [
   ProcessConfig(
     proc_name="plannerd",
     pubs=["modelV2", "carControl", "carState", "controlsState", "radarState", "selfdriveState"],
-    subs=["longitudinalPlan"],
+    subs=["longitudinalPlan", "driverAssistance"],
     ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"],
     init_callback=get_car_params_callback,
     should_recv_callback=FrequencyBasedRcvCallback("modelV2"),

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-7f9255ef41d0257f8665d441c666cb2721cdf3ee
+fc1344b16b802cdb4ec542791f3b4d76a82da68b