Revert "Reapply "Tomb Raider 6""

This reverts commit f3de8169b9.

selfdrive/controls/lib/drive_helpers.py

@@ -1,10 +1,9 @@
 import math
-import numpy as np
 
 from cereal import car, log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.realtime import DT_CTRL, DT_MDL
+from openpilot.common.realtime import DT_CTRL
 
 # WARNING: this value was determined based on the model's training distribution,
 #          model predictions above this speed can be unpredictable
@@ -179,9 +178,6 @@ def apply_center_deadzone(error, deadzone):
 def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
-def smooth_value(val, prev_val, tau, dt=DT_MDL):
-  alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
-  return alpha * val + (1 - alpha) * prev_val
 
 def clip_curvature(v_ego, prev_curvature, new_curvature, planner_curves):
   if planner_curves:
@@ -212,29 +208,3 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
     vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
     return float(vel_err)
   return 0.0
-
-
-def get_accel_from_plan_tomb_raider(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
-  if len(speeds) == len(t_idxs):
-    v_now = speeds[0]
-    a_now = accels[0]
-    v_target = np.interp(action_t, t_idxs, speeds)
-    a_target = 2 * (v_target - v_now) / (action_t) - a_now
-    v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
-  else:
-    v_target = 0.0
-    v_target_1sec = 0.0
-    a_target = 0.0
-  should_stop = (v_target < vEgoStopping and
-                 v_target_1sec < vEgoStopping)
-  return a_target, should_stop
-
-def curv_from_psis(psi_target, psi_rate, vego, action_t):
-  vego = np.clip(vego, MIN_SPEED, np.inf)
-  curv_from_psi = psi_target / (vego * action_t)
-  return 2*curv_from_psi - psi_rate / vego
-
-def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
-  psi_target = np.interp(action_t, t_idxs, yaws)
-  psi_rate = yaw_rates[0]
-  return curv_from_psis(psi_target, psi_rate, vego, action_t)

selfdrive/controls/lib/longitudinal_planner.py

@@ -13,7 +13,7 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU
-from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error, get_accel_from_plan_tomb_raider
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, CONTROL_N, get_speed_error
 from openpilot.common.swaglog import cloudlog
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
@@ -203,12 +203,8 @@ class LongitudinalPlanner:
       throttle_prob = 1.0
     return x, v, a, j, throttle_prob
 
-  def update(self, radarless_model, tomb_raider, sm, frogpilot_toggles):
-    if tomb_raider:
-      self.mpc.mode = 'acc'
-      self.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
-    else:
-      self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
+  def update(self, radarless_model, sm, frogpilot_toggles):
+    self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -298,7 +294,7 @@ class LongitudinalPlanner:
     self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-  def publish(self, classic_model, tomb_raider, sm, pm, frogpilot_toggles):
+  def publish(self, classic_model, sm, pm, frogpilot_toggles):
     plan_send = messaging.new_message('longitudinalPlan')
 
     plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
@@ -318,25 +314,12 @@ class LongitudinalPlanner:
 
     if classic_model:
       a_target, should_stop = get_accel_from_plan_classic(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels, vEgoStopping=frogpilot_toggles.vEgoStopping)
-    elif tomb_raider:
-      action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
-      output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan_tomb_raider(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
-                                                                                    action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
-      output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
-      output_should_stop_e2e = sm['modelV2'].action.shouldStop
-
-      if self.mode == 'acc':
-        a_target = output_a_target_mpc
-        should_stop = output_should_stop_mpc
-      else:
-        a_target = min(output_a_target_mpc, output_a_target_e2e)
-        should_stop = output_should_stop_e2e or output_should_stop_mpc
     else:
       action_t = self.CP.longitudinalActuatorDelay + DT_MDL
       a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
                                                   action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping)
-    longitudinalPlan.aTarget = float(a_target)
-    longitudinalPlan.shouldStop = bool(should_stop)
+    longitudinalPlan.aTarget = a_target
+    longitudinalPlan.shouldStop = should_stop
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = self.allow_throttle
 

selfdrive/controls/plannerd.py

@@ -39,13 +39,12 @@ def plannerd_thread():
 
   classic_model = frogpilot_toggles.classic_model
   radarless_model = frogpilot_toggles.radarless_model
-  tomb_raider = frogpilot_toggles.model == "tomb-raider"
 
   while True:
     sm.update()
     if sm.updated['modelV2']:
-      longitudinal_planner.update(radarless_model, tomb_raider, sm, frogpilot_toggles)
-      longitudinal_planner.publish(classic_model, tomb_raider, sm, pm, frogpilot_toggles)
+      longitudinal_planner.update(radarless_model, sm, frogpilot_toggles)
+      longitudinal_planner.publish(classic_model, sm, pm, frogpilot_toggles)
       publish_ui_plan(sm, pm, longitudinal_planner)
 
     # Update FrogPilot parameters