Simplify lane change logic (#38229)

* desire cleanups

* remove the finalize timeout

---------

Co-authored-by: Robbe Derks <robbe.derks@gmail.com>

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -7,35 +7,13 @@ LaneChangeDirection = log.LaneChangeDirection
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
-
-DESIRES = {
-  LaneChangeDirection.none: {
-    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.Desire.none,
-    LaneChangeState.laneChangeFinishing: log.Desire.none,
-  },
-  LaneChangeDirection.left: {
-    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft,
-    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft,
-  },
-  LaneChangeDirection.right: {
-    LaneChangeState.off: log.Desire.none,
-    LaneChangeState.preLaneChange: log.Desire.none,
-    LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight,
-    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight,
-  },
-}
-
+LANE_CHANGE_START_TIME = 0.5
 
 class DesireHelper:
   def __init__(self):
     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
     self.desire = log.Desire.none
 
@@ -51,15 +29,14 @@ class DesireHelper:
     if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
       self.lane_change_state = LaneChangeState.off
       self.lane_change_direction = LaneChangeDirection.none
+      self.lane_change_timer = 0.0
     else:
-      # LaneChangeState.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
+        self.lane_change_timer = 0.0
         # Initialize lane change direction to prevent UI alert flicker
         self.lane_change_direction = self.get_lane_change_direction(carstate)
 
-      # LaneChangeState.preLaneChange
       elif self.lane_change_state == LaneChangeState.preLaneChange:
         # Update lane change direction
         self.lane_change_direction = self.get_lane_change_direction(carstate)
@@ -74,35 +51,28 @@ class DesireHelper:
         if not one_blinker or below_lane_change_speed:
           self.lane_change_state = LaneChangeState.off
           self.lane_change_direction = LaneChangeDirection.none
+          self.lane_change_timer = 0.0
         elif torque_applied and not blindspot_detected:
           self.lane_change_state = LaneChangeState.laneChangeStarting
+          self.lane_change_timer = 0.0
 
-      # LaneChangeState.laneChangeStarting
       elif self.lane_change_state == LaneChangeState.laneChangeStarting:
-        # fade out over .5s
-        self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
+        self.lane_change_timer += DT_MDL
 
-        # 98% certainty
-        if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
-          self.lane_change_state = LaneChangeState.laneChangeFinishing
-
-      # LaneChangeState.laneChangeFinishing
-      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 self.lane_change_ll_prob > 0.99:
-          self.lane_change_direction = LaneChangeDirection.none
+        if lane_change_prob < 0.02 and self.lane_change_timer >= LANE_CHANGE_START_TIME:
+          self.lane_change_timer = 0.0
           if one_blinker:
             self.lane_change_state = LaneChangeState.preLaneChange
+            self.lane_change_direction = self.get_lane_change_direction(carstate)
           else:
             self.lane_change_state = LaneChangeState.off
-
-    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
-      self.lane_change_timer = 0.0
-    else:
-      self.lane_change_timer += DT_MDL
+            self.lane_change_direction = LaneChangeDirection.none
 
     self.prev_one_blinker = one_blinker
 
-    self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+    self.desire = log.Desire.none
+    if self.lane_change_state == LaneChangeState.laneChangeStarting:
+      if self.lane_change_direction == LaneChangeDirection.left:
+        self.desire = log.Desire.laneChangeLeft
+      elif self.lane_change_direction == LaneChangeDirection.right:
+        self.desire = log.Desire.laneChangeRight