From 90b77e85c3d4a19ab96c30ebbd4ac3380635e554 Mon Sep 17 00:00:00 2001 From: kegman Date: Fri, 30 Nov 2018 16:00:39 -0500 Subject: [PATCH] Reduce tailgating for one bar distance at surface street speeds & code cleanup (#43) * Gernby's Lane width filter fix (#41) * Fix lane width filtering * Fixed lane width filtering * CAMERA_OFFSET = 0.08 * Minimum distance for one bar distance setting * Don't tailgate at slow speeds with 1 bar distance * x_lead < 5 only * x_lead < 5 syntax fix * self.v_rel < -1 or (x_lead < 10 and self.v_rel >= -1 and self.v_rel < 1) * Tail gating auto distance change * x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.2 * self.lastTR > 0 re-adding reinitialization * Revert "Gernby's Lane width filter fix (#41)" This reverts commit d081cfd8a05d2eb32e0148d477c806e65662dea7. * Increase min distance to 10m for auto distance * Auto distance code cleanup * Add lead car fast approaching autodistance * Auto distance Code cleanup * Don't re-initialize MPC if already initialized * self.lastTR != instead of > * Initialize TR = 1.8 * include if statements vs just assigning boolean * switch to 1 and 0 for TRUE AND FALSE * Revert "switch to 1 and 0 for TRUE AND FALSE" This reverts commit 58dcf718cdc3fca636229bf29942669670cb4487. * Revert "include if statements vs just assigning boolean" This reverts commit da7b5fa5088d62e78ba57d9e4c01e89160e77b9e. * Revert "Initialize TR = 1.8" This reverts commit f9c2571faeb0acaab818bcff146a2a397c0f07ff. * Initialize TR=1.8 inside override loop * Cleanup TR assignment for override loop * Code Cleanup * Syntax error * Syntax error * if statements for autodistance vars * tr-last assignment indent * CS.readdistancelines assignment to self.tr_last * remove else MPC init * Flatten out elif ladder for autodistance * Removed shrinking_fast stopping for now Braking is too sudden with 3 bar distance when radar detects car. Shrinking gap takes care of this with 2 bar distance. * Code Cleanup --- selfdrive/controls/lib/pathplanner.py | 2 +- selfdrive/controls/lib/planner.py | 74 +++++++++++++++++---------- 2 files changed, 49 insertions(+), 27 deletions(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 712c3031edb589..90e37676e79e2e 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,7 +1,7 @@ from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -CAMERA_OFFSET = 0.06 # m from center car to camera +CAMERA_OFFSET = 0.08 # m from center car to camera class PathPlanner(object): def __init__(self): diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 6c99b12283a6eb..6cee1a2d95b9bc 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -151,6 +151,10 @@ def __init__(self, mpc_id, live_longitudinal_mpc): self.override = False # for one bar distance at low speeds - introduce early braking and hysteresis for resume follow self.lastTR = 2 self.last_cloudlog_t = 0.0 + self.v_rel = 10 + self.tailgating = 0 + self.street_speed = 0 + self.lead_car_gap_shrinking = 0 def send_mpc_solution(self, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) @@ -221,38 +225,56 @@ def update(self, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - # Override with 3 bar distance profile if using one bar distance - hysteresis for safety - #if self.override and CS.aEgo >= 0: - #self.override = False # When car starts accelerating, resume one bar distance profile - - if CS.readdistancelines == 1 and CS.vEgo < 18.06 and (v_lead - CS.vEgo) < -1: - #self.override = True # If we start braking, then keep 3 bar distance profile until car starts to accelerate - TR=1.8 - #if self.lastTR > 0: - #self.libmpc.init(MPC_COST_LONG.TTC, 0.1, PC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - #self.lastTR = 0 + # Calculate conditions + self.v_rel = v_lead - CS.vEgo # calculate relative velocity vs lead car + + # Defining some variables to make the logic more human readable for auto distance override below + # Is the car tailgating the lead car? + if x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.25: + self.tailgating = 1 else: - if CS.readdistancelines == 2: - TR=1.8 # 20m at 40km/hr - if CS.readdistancelines != self.lastTR: + self.tailgating = 0 + + # Is the car running surface street speeds? + if CS.vEgo < 18.06: + self.street_speed = 1 + else: + self.street_speed = 0 + + # Is the gap from the lead car shrinking? + if self.v_rel < -1: + self.lead_car_gap_shrinking = 1 + else: + self.lead_car_gap_shrinking = 0 + + # Adjust distance from lead car when distance button pressed + if CS.readdistancelines == 1: + # If one bar distance, auto set to 2 bar distance under current conditions to prevent rear ending lead car + if self.street_speed and (self.lead_car_gap_shrinking or self.tailgating): + TR=1.8 + if self.lastTR != 0: self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.lastTR = CS.readdistancelines - - elif CS.readdistancelines == 1: + self.lastTR = 0 + else: TR=0.9 # 10m at 40km/hr if CS.readdistancelines != self.lastTR: self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.lastTR = CS.readdistancelines + self.lastTR = CS.readdistancelines + + elif CS.readdistancelines == 2: + TR=1.8 # 20m at 40km/hr + if CS.readdistancelines != self.lastTR: + self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = CS.readdistancelines + + elif CS.readdistancelines == 3: + TR=2.7 # 30m at 40km/hr + if CS.readdistancelines != self.lastTR: + self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = CS.readdistancelines - elif CS.readdistancelines == 3: - TR=2.7 # 30m at 40km/hr - if CS.readdistancelines != self.lastTR: - self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) - self.lastTR = CS.readdistancelines - - else: - TR=1.8 # if readdistancelines = 0 - + else: + TR=1.8 # if readdistancelines = 0 n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR) duration = int((sec_since_boot() - t) * 1e9)