From fe9ccb27b12c9d3788bed8a402293985f5eb0448 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 27 Jan 2020 16:19:22 -0800 Subject: [PATCH] Remember lane change direction when blinker turns off --- selfdrive/controls/lib/pathplanner.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 8a6e5286f3..5759ee55f7 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -55,6 +55,7 @@ class PathPlanner(): self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.prev_one_blinker = False @@ -89,21 +90,21 @@ class PathPlanner(): self.LP.parse_model(sm['model']) # Lane change logic - lane_change_direction = LaneChangeDirection.none one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + if sm['carState'].leftBlinker: + self.lane_change_direction = LaneChangeDirection.left + elif sm['carState'].rightBlinker: + self.lane_change_direction = LaneChangeDirection.right + if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none else: - if sm['carState'].leftBlinker: - lane_change_direction = LaneChangeDirection.left - elif sm['carState'].rightBlinker: - lane_change_direction = LaneChangeDirection.right - torque_applied = sm['carState'].steeringPressed and \ - ((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \ - (sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right)) + ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \ + (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob @@ -137,7 +138,7 @@ class PathPlanner(): self.prev_one_blinker = one_blinker - desire = DESIRES[lane_change_direction][self.lane_change_state] + desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: @@ -215,7 +216,7 @@ class PathPlanner(): plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state - plan_send.pathPlan.laneChangeDirection = lane_change_direction + plan_send.pathPlan.laneChangeDirection = self.lane_change_direction pm.send('pathPlan', plan_send)