From 1de23f3d1078c86c6e989abeee31bf27efa3569e Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Mon, 6 May 2019 22:19:00 +0200 Subject: [PATCH] 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 old-commit-hash: f5044670fa7665bacab49d438e9631191312b833 --- cereal/car.capnp | 2 ++ selfdrive/car/toyota/carcontroller.py | 5 +++-- selfdrive/car/toyota/interface.py | 3 ++- selfdrive/car/toyota/toyotacan.py | 7 ++++--- selfdrive/controls/controlsd.py | 4 ++++ selfdrive/controls/lib/pathplanner.py | 5 ++++- 6 files changed, 19 insertions(+), 7 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 8fb3128676..2915d68713 100644 --- a/cereal/car.capnp +++ b/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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 8197a7922a..d69636d32d 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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)) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 8b6cb0a012..0fe0d0821a 100755 --- a/selfdrive/car/toyota/interface.py +++ b/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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 720c0ef60a..85b8dce8d7 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 60d3536ffe..d07ccdccaf 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 826b6dce1c..40628ad09f 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/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()