diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 39bb57463b..c2fb6c8aac 100644 --- a/CHANGELOGS.md +++ b/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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1c0e2419db..a0c0c7f241 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index 07a6c790ce..d2ac10cd3b 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/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) diff --git a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc index dbe2ca4e35..d75ef3d205 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/sunnypilot_settings.cc +++ b/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", }, diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 70dbbf056f..0b2e7af46b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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