From af5f99d7cfcc37574d893a86a1949f4b5bbbbe8f Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Mon, 16 Mar 2020 16:19:01 -0700 Subject: [PATCH] fade ll out and in (#1246) * fade ll out and in * bug fixes * pretty important fixes * safer in case model misses desire input * Safer float compares Co-authored-by: Willem Melching --- selfdrive/controls/lib/pathplanner.py | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 07d06fce55..0e8a4effe3 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -22,20 +22,24 @@ DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, }, LaneChangeDirection.left: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, }, LaneChangeDirection.right: { LaneChangeState.off: log.PathPlan.Desire.none, LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, }, } + def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay @@ -55,6 +59,7 @@ class PathPlanner(): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False def setup_mpc(self): @@ -110,19 +115,30 @@ class PathPlanner(): # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # starting + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + # fade out lanelines over 1s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL, 0.0) + # 98% certainty + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.5: - if one_blinker: + elif self.lane_change_state == LaneChangeState.laneChangeFinishing: + # fade in laneline over 1s + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange - else: + elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: @@ -136,8 +152,8 @@ class PathPlanner(): # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: - self.LP.l_prob = 0. - self.LP.r_prob = 0. + self.LP.l_prob *= self.lane_change_ll_prob + self.LP.r_prob *= self.lane_change_ll_prob self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) else: self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost)