V-TSC: New implementation

CHANGELOGS.md

@@ -1,5 +1,8 @@
 sunnypilot - 0.9.6.1 (2023-xx-xx)
 ========================
+* UPDATED: Vision-based Turn Speed Control (V-TSC) implementation
+  * Refactored implementation thanks to pfeiferj!
+  * More accurate and consistent velocity calculation to achieve smoother longitudinal control in curves
 * DISABLED: Toyota: ZSS support
 
 sunnypilot - 0.9.5.2 (2023-12-07)

selfdrive/controls/lib/longitudinal_planner.py

@@ -153,8 +153,8 @@ class LongitudinalPlanner:
 
     # Get active solutions for custom long mpc.
     self.cruise_source, v_cruise_sol = self.cruise_solutions(
-      not reset_state and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed), self.v_desired_filter.x,
-      self.a_desired, v_cruise, sm)
+      prev_accel_constraint and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed),
+      self.v_desired_filter.x, self.a_desired, v_cruise, sm)
 
     # clip limits, cannot init MPC outside of bounds
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
@@ -217,7 +217,7 @@ class LongitudinalPlanner:
     longitudinalPlanSP.e2eX = self.mpc.e2e_x.tolist()
 
     longitudinalPlanSP.visionTurnControllerState = self.vision_turn_controller.state
-    longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_turn)
+    longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_target)
     longitudinalPlanSP.visionCurrentLatAcc = float(self.vision_turn_controller.current_lat_acc)
     longitudinalPlanSP.visionMaxPredLatAcc = float(self.vision_turn_controller.max_pred_lat_acc)
 
@@ -241,7 +241,7 @@ class LongitudinalPlanner:
 
   def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm):
     # Update controllers
-    self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm)
+    self.vision_turn_controller.update(enabled, v_ego, v_cruise, sm)
     self.events = Events()
     self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.CP, self.events)
     self.turn_speed_controller.update(enabled, v_ego, a_ego, sm)
@@ -250,7 +250,7 @@ class LongitudinalPlanner:
     v_solutions = {'cruise': v_cruise}
 
     if self.vision_turn_controller.is_active:
-      v_solutions['turn'] = self.vision_turn_controller.v_turn
+      v_solutions['turn'] = self.vision_turn_controller.v_target
 
     if self.speed_limit_controller.is_active:
       v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted

selfdrive/controls/lib/vision_turn_controller.py

@@ -1,85 +1,26 @@
+# PFEIFER - VTSC
+
+# Acknowledgements:
+# Past versions of this code were based on the move-fast teams vtsc
+# implementation. (https://github.com/move-fast/openpilot) Huge thanks to them
+# for their initial implementation. I also used sunnypilot as a reference.
+# (https://github.com/sunnyhaibin/sunnypilot) Big thanks for sunny's amazing work
+
 import numpy as np
-import math
 import time
 from cereal import custom
-from common.numpy_fast import interp
-from common.params import Params
-from common.conversions import Conversions as CV
-from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.sunnypilot.helpers import debug
 
+TARGET_LAT_A = 1.9  # m/s^2
+MIN_TARGET_V = 5  # m/s
 
-_MIN_V = 5.6  # Do not operate under 20km/h
-
-_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
-_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
-
-_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning turn state.
-
-_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
-_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger end of turn cycle.
-
-_EVAL_STEP = 5.  # mts. Resolution of the curvature evaluation.
-_EVAL_START = 20.  # mts. Distance ahead where to start evaluating vision curvature.
-_EVAL_LENGHT = 150.  # mts. Distance ahead where to stop evaluating vision curvature.
-_EVAL_RANGE = np.arange(_EVAL_START, _EVAL_LENGHT, _EVAL_STEP)
-
-_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
-
-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Confortble acceleration to regain speed while leaving a turn.
-
-_MIN_LANE_PROB = 0.6  # Minimum lanes probability to allow curvature prediction based on lanes.
-
-_DEBUG = False
-
-
-def _debug(msg):
-  if not _DEBUG:
-    return
-  print(msg)
-
+PARAMS_UPDATE_PERIOD = 5.
 
 VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState
 
 
-def eval_curvature(poly, x_vals):
-  """
-  This function returns a vector with the curvature based on path defined by `poly`
-  evaluated on distance vector `x_vals`
-  """
-  # https://en.wikipedia.org/wiki/Curvature#  Local_expressions
-  def curvature(x):
-    a = abs(2 * poly[1] + 6 * poly[0] * x) / (1 + (3 * poly[0] * x**2 + 2 * poly[1] * x + poly[2])**2)**(1.5)
-    return a
-
-  return np.vectorize(curvature)(x_vals)
-
-
-def eval_lat_acc(v_ego, x_curv):
-  """
-  This function returns a vector with the lateral acceleration based
-  for the provided speed `v_ego` evaluated over curvature vector `x_curv`
-  """
-
-  def lat_acc(curv):
-    a = v_ego**2 * curv
-    return a
-
-  return np.vectorize(lat_acc)(x_curv)
-
-
 def _description_for_state(turn_controller_state):
   if turn_controller_state == VisionTurnControllerState.disabled:
     return 'DISABLED'
@@ -91,19 +32,19 @@ def _description_for_state(turn_controller_state):
     return 'LEAVING'
 
 
-class VisionTurnController():
+class VisionTurnController:
   def __init__(self, CP):
     self._params = Params()
     self._CP = CP
     self._op_enabled = False
     self._gas_pressed = False
     self._is_enabled = self._params.get_bool("TurnVisionControl")
-    self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator")
     self._last_params_update = 0.
-    self._v_cruise_setpoint = 0.
     self._v_ego = 0.
-    self._a_ego = 0.
-    self._v_overshoot = 0.
+    self._v_target = MIN_TARGET_V
+    self._current_lat_acc = 0.
+    self._max_pred_lat_acc = 0.
+    self._v_cruise = 0.
     self._state = VisionTurnControllerState.disabled
 
     self._reset()
@@ -115,17 +56,14 @@ class VisionTurnController():
   @state.setter
   def state(self, value):
     if value != self._state:
-      _debug(f'TVC: TurnVisionController state: {_description_for_state(value)}')
+      debug(f"V-TSC: VisionTurnControllerState state: {_description_for_state(value)}")
       if value == VisionTurnControllerState.disabled:
         self._reset()
     self._state = value
 
   @property
-  def v_turn(self):
-    if not self.is_active:
-      return self._v_cruise_setpoint
-    return self._v_overshoot if self._lat_acc_overshoot_ahead \
-      else self._v_ego
+  def v_target(self):
+    return self._v_target
 
   @property
   def is_active(self):
@@ -141,126 +79,53 @@ class VisionTurnController():
 
   def _reset(self):
     self._current_lat_acc = 0.
-    self._max_v_for_current_curvature = 0.
     self._max_pred_lat_acc = 0.
-    self._v_overshoot_distance = 200.
-    self._lat_acc_overshoot_ahead = False
 
   def _update_params(self):
     t = time.monotonic()
-    if t > self._last_params_update + 5.0:
+    if t > self._last_params_update + PARAMS_UPDATE_PERIOD:
       self._is_enabled = self._params.get_bool("TurnVisionControl")
       self._last_params_update = t
 
   def _update_calculations(self, sm):
-    # Get path polynomial approximation for curvature estimation from model data.
-    path_poly = None
-    model_data = sm['modelV2'] if sm.valid.get('modelV2', False) else None
-    lat_planner_data = sm['lateralPlanSP'] if sm.valid.get('lateralPlanSP', False) else None
-
-    # 1. When the probability of lanes is good enough, compute polynomial from lanes as they are way more stable
-    # on current mode than drving path.
-    if model_data is not None and len(model_data.laneLines) == 4 and len(model_data.laneLines[0].t) == TRAJECTORY_SIZE:
-      ll_x = model_data.laneLines[1].x  # left and right ll x is the same
-      lll_y = np.array(model_data.laneLines[1].y)
-      rll_y = np.array(model_data.laneLines[2].y)
-      l_prob = model_data.laneLineProbs[1]
-      r_prob = model_data.laneLineProbs[2]
-      lll_std = model_data.laneLineStds[1]
-      rll_std = model_data.laneLineStds[2]
-
-      # Reduce reliance on lanelines that are too far apart or will be in a few seconds
-      width_pts = rll_y - lll_y
-      prob_mods = []
-      for t_check in [0.0, 1.5, 3.0]:
-        width_at_t = interp(t_check * (self._v_ego + 7), ll_x, width_pts)
-        prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
-      mod = min(prob_mods)
-      l_prob *= mod
-      r_prob *= mod
-
-      # Reduce reliance on uncertain lanelines
-      l_std_mod = interp(lll_std, [.15, .3], [1.0, 0.0])
-      r_std_mod = interp(rll_std, [.15, .3], [1.0, 0.0])
-      l_prob *= l_std_mod
-      r_prob *= r_std_mod
-
-      # Find path from lanes as the average center lane only if min probability on both lanes is above threshold.
-      if l_prob > _MIN_LANE_PROB and r_prob > _MIN_LANE_PROB:
-        c_y = width_pts / 2 + lll_y
-        path_poly = np.polyfit(ll_x, c_y, 3)
-
-    # 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as
-    # provided by `lateralPlanner`.
-    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \
-       and lat_planner_data.dPathWLinesX[0] > 0:
-      path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3)
-
-    # 3. If no polynomial derived from lanes or driving path, then provide a straight line poly.
-    if path_poly is None:
-      path_poly = np.array([0., 0., 0., 0.])
+    rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+    vel_plan = np.array(sm['modelV2'].velocity.x)
 
     current_curvature = abs(
       sm['carState'].steeringAngleDeg * CV.DEG_TO_RAD / (self._CP.steerRatio * self._CP.wheelbase))
     self._current_lat_acc = current_curvature * self._v_ego**2
-    self._max_v_for_current_curvature = math.sqrt(_A_LAT_REG_MAX / current_curvature) if current_curvature > 0 \
-      else V_CRUISE_MAX * CV.KPH_TO_MS
 
-    pred_curvatures = eval_curvature(path_poly, _EVAL_RANGE)
-    max_pred_curvature = np.amax(pred_curvatures)
-    self._max_pred_lat_acc = self._v_ego**2 * max_pred_curvature
+    # get the maximum lat accel from the model
+    predicted_lat_accels = rate_plan * vel_plan
+    self._max_pred_lat_acc = np.amax(predicted_lat_accels)
 
-    max_curvature_for_vego = _A_LAT_REG_MAX / max(self._v_ego, 0.1)**2
-    lat_acc_overshoot_idxs = np.nonzero(pred_curvatures >= max_curvature_for_vego)[0]
-    self._lat_acc_overshoot_ahead = len(lat_acc_overshoot_idxs) > 0
+    # get the maximum curve based on the current velocity
+    v_ego = max(self._v_ego, 0.1)  # ensure a value greater than 0 for calculations
+    max_curve = self.max_pred_lat_acc / (v_ego**2)
 
-    if self._lat_acc_overshoot_ahead:
-      self._v_overshoot = min(math.sqrt(_A_LAT_REG_MAX / max_pred_curvature), self._v_cruise_setpoint)
-      self._v_overshoot_distance = max(lat_acc_overshoot_idxs[0] * _EVAL_STEP + _EVAL_START, _EVAL_STEP)
-      _debug(f'TVC: High LatAcc. Dist: {self._v_overshoot_distance:.2f}, v: {self._v_overshoot * CV.MS_TO_KPH:.2f}')
+    # Get the target velocity for the maximum curve
+    self._v_target = (TARGET_LAT_A / max_curve) ** 0.5
+    self._v_target = max(self.v_target, MIN_TARGET_V)
 
   def _state_transition(self):
-    # In any case, if system is disabled or the feature is disabeld or gas is pressed, disable.
-    if not self._op_enabled or not self._is_enabled or (self._gas_pressed and self._disengage_on_accelerator):
+    if not self._op_enabled or not self._is_enabled or self._gas_pressed:
       self.state = VisionTurnControllerState.disabled
       return
 
     # DISABLED
     if self.state == VisionTurnControllerState.disabled:
-      # Do not enter a turn control cycle if speed is low.
-      if self._v_ego <= _MIN_V:
-        pass
-      # If substantial lateral acceleration is predicted ahead, then move to Entering turn state.
-      elif self._max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.entering
-    # ENTERING
-    elif self.state == VisionTurnControllerState.entering:
-      # Transition to Turning if current lateral acceleration is over the threshold.
-      if self._current_lat_acc >= _TURNING_LAT_ACC_TH:
+      if self._v_cruise > self.v_target:
         self.state = VisionTurnControllerState.turning
-      # Abort if the predicted lateral acceleration drops
-      elif self._max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.disabled
     # TURNING
     elif self.state == VisionTurnControllerState.turning:
-      # Transition to Leaving if current lateral acceleration drops drops below threshold.
-      if self._current_lat_acc <= _LEAVING_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.leaving
-    # LEAVING
-    elif self.state == VisionTurnControllerState.leaving:
-      # Transition back to Turning if current lateral acceleration goes back over the threshold.
-      if self._current_lat_acc >= _TURNING_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.turning
-      # Finish if current lateral acceleration goes below threshold.
-      elif self._current_lat_acc < _FINISH_LAT_ACC_TH:
+      if not (self._v_cruise > self.v_target):
         self.state = VisionTurnControllerState.disabled
 
-  def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm):
+  def update(self, enabled, v_ego, v_cruise, sm):
     self._op_enabled = enabled
     self._gas_pressed = sm['carState'].gasPressed
     self._v_ego = v_ego
-    self._a_ego = a_ego
-    self._v_cruise_setpoint = v_cruise_setpoint
+    self._v_cruise = v_cruise
 
     self._update_params()
     self._update_calculations(sm)

selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc

@@ -26,7 +26,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) {
     },
     {
       "TurnVisionControl",
-      tr("Enable Vision Based Turn Speed Control (V-TSC)"),
+      tr("Enable Vision-based Turn Speed Control (V-TSC)"),
       tr("Use vision path predictions to estimate the appropriate speed to drive through turns ahead."),
       "../assets/offroad/icon_blank.png",
     },

selfdrive/ui/qt/onroad.cc

@@ -796,6 +796,8 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
 
   e2eStatus = chime_prompt;
   e2eState = e2eLStatus;
+
+  experimental_btn->setVisible(!showVTC);
 }
 
 void AnnotatedCameraWidget::drawHud(QPainter &p) {
@@ -948,7 +950,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
 
     // V-TSC
     if (showDebugUI && showVTC) {
-      drawVisionTurnControllerUI(p, rect().right() - 184 - UI_BORDER_SIZE, int(UI_BORDER_SIZE * 1.5), 184, vtcColor, vtcSpeed, 100);
+      drawVisionTurnControllerUI(p, rect().right() - 184 - (UI_BORDER_SIZE * 1.5), int(UI_BORDER_SIZE * 1.5), 184, vtcColor, vtcSpeed, 100);
     }
 
     // Bottom bar road name