Add lane departure warning on dashboard for Toyota (#605)

* Add lane departure alert in controlsd

* Need init values for LDA

* Add lane departure in interface.py

* Include LDA in CarControler

* Add logic for LDA in toyotacan

* Add speed condition and comments for LDA

* Correct right CS.vEgo

* Correct rPoly spelling

* Add left and rightLaneDepart to HUDControl in car.capnp

* Add left and rightLane_Depart in UI function

* set controlsd priority

* revert

* There must be a line to depart from

* Include changes from @pd0wm

* Remove redundant False allocation

leftLaneDepart and rightLaneDepart as False by default according to @pd0wm

* Modify variable names

right_lane_depart and left_lane_depart to conform with python naming convention

* Modify variable names

right_lane_depart and left_lane_depart to conform with python naming convention

* Wrap lane departure warning in one bool

cereal/car.capnp

@@ -247,6 +247,8 @@ struct CarControl {
     audibleAlert @5: AudibleAlert;
     rightLaneVisible @6: Bool;
     leftLaneVisible @7: Bool;
+    rightLaneDepart @8: Bool;
+    leftLaneDepart @9: Bool;
 
     enum VisualAlert {
       # these are the choices from the Honda

selfdrive/car/toyota/carcontroller.py

@@ -125,7 +125,8 @@ class CarController(object):
     self.packer = CANPacker(dbc_name)
 
   def update(self, sendcan, enabled, CS, frame, actuators,
-             pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead):
+             pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
+             left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
     # *** compute control surfaces ***
 
@@ -246,7 +247,7 @@ class CarController(object):
       send_ui = False
 
     if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
-      can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line))
+      can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
 
     if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
       can_sends.append(create_fcw_command(self.packer, fcw))

selfdrive/car/toyota/interface.py

@@ -369,7 +369,8 @@ class CarInterface(object):
     self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
                    c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
                    c.hudControl.audibleAlert, self.forwarding_camera,
-                   c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible)
+                   c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible, 
+                   c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
 
     self.frame += 1
     return False

selfdrive/car/toyota/toyotacan.py

@@ -89,10 +89,11 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, steer, sound1, sound2, left_line, right_line):
+def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart):
   values = {
-    "RIGHT_LINE": 1 if right_line else 2,
-    "LEFT_LINE": 1 if left_line else 2,
+    "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
+    "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
+    "BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0,
     "SET_ME_X0C": 0x0c,
     "SET_ME_X2C": 0x2c,
     "SET_ME_X38": 0x38,

selfdrive/controls/controlsd.py

@@ -307,6 +307,10 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
     CC.hudControl.leadVisible = plan.hasLead
     CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
     CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
+    if len(list(path_plan.pathPlan.rPoly)) == 4:
+      CC.hudControl.rightLaneDepart = bool(path_plan.pathPlan.rPoly[3] > -1.11 and not CS.rightBlinker and CS.vEgo > 12.5 and path_plan.pathPlan.rProb > 0.5) # Speed needs to be above 12.5m/s for LDA and only if blinker if off
+    if len(list(path_plan.pathPlan.lPoly)) == 4:
+      CC.hudControl.leftLaneDepart = bool(path_plan.pathPlan.lPoly[3] < 1.05 and not CS.leftBlinker and CS.vEgo > 12.5 and path_plan.pathPlan.lProb > 0.5) # CAMERA_OFFSET 6cm making it to detect if line is within 15cm of the wheel
     CC.hudControl.visualAlert = AM.visual_alert
     CC.hudControl.audibleAlert = AM.audible_alert
 

selfdrive/controls/lib/pathplanner.py

@@ -20,7 +20,10 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
 class PathPlanner(object):
   def __init__(self, CP):
     self.MP = ModelParser()
-
+    
+    self.l_poly = [0., 0., 0., 0.]
+    self.r_poly = [0., 0., 0., 0.]
+    
     self.last_cloudlog_t = 0
 
     context = zmq.Context()