#34331

1

@@ -0,0 +1,48 @@
+import math
+
+from cereal import log
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+
+class LatControlPID(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                             k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+
+  def reset(self):
+    super().reset()
+    self.pid.reset()
+
+  def update(self, active, CS, VM, params, steer_limited, desired_curvature, calibrated_pose):
+    pid_log = log.ControlsState.LateralPIDState.new_message()
+    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
+
+    pid_log.steeringAngleDesiredDeg = angle_steers_des
+    pid_log.angleError = error
+    if not active:
+      output_steer = 0.0
+      pid_log.active = False
+      self.pid.reset()
+    else:
+      # offset does not contribute to resistive torque
+      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+
+      output_steer = self.pid.update(error, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo)
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(output_steer)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited))
+
+    return output_steer, angle_steers_des, pid_log

common/pid.py

@@ -1,9 +1,6 @@
 import numpy as np
 from numbers import Number
 
-from openpilot.common.numpy_fast import clip, interp
-
-
 class PIDController:
   def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
     self._k_p = k_p
@@ -28,15 +25,15 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308,
 
   @property
   def k_p(self):
-    return interp(self.speed, self._k_p[0], self._k_p[1])
+    return float(np.interp(self.speed, self._k_p[0], self._k_p[1]))
 
   @property
   def k_i(self):
-    return interp(self.speed, self._k_i[0], self._k_i[1])
+    return float(np.interp(self.speed, self._k_i[0], self._k_i[1]))
 
   @property
   def k_d(self):
-    return interp(self.speed, self._k_d[0], self._k_d[1])
+    return float(np.interp(self.speed, self._k_d[0], self._k_d[1]))
 
   @property
   def error_integral(self):
@@ -64,10 +61,10 @@ def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0
 
         # Clip i to prevent exceeding control limits
         control_no_i = self.p + self.d + self.f
-        control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
-        self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
+        control_no_i = float(np.clip(control_no_i, self.neg_limit, self.pos_limit))
+        self.i = float(np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i))
 
     control = self.p + self.i + self.d + self.f
 
-    self.control = clip(control, self.neg_limit, self.pos_limit)
+    self.control = float(np.clip(control, self.neg_limit, self.pos_limit))
     return self.control

common/tests/test_numpy_fast.py

@@ -1,21 +1,23 @@
 import numpy as np
 
-from openpilot.common.numpy_fast import interp
-
 
 class TestInterp:
-  def test_correctness_controls(self):
-    _A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.])
-    _A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
-    v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39,
-                 39.999999, 40, 41]
+    def test_correctness_controls(self):
+        _A_CRUISE_MIN_BP = np.asarray([0., 5., 10., 20., 40.])
+        _A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
+        v_ego_arr = [-1, -1e-12, 0, 4, 5, 6, 7, 10, 11, 15.2, 20, 21, 39,
+                     39.999999, 40, 41]
+
+        # Use np.interp directly for arrays
+        expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+        actual = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
 
-    expected = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-    actual = interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
+        # Assert that the arrays are equal
+        np.testing.assert_array_equal(actual, expected)
 
-    np.testing.assert_equal(actual, expected)
+        # Test for individual scalar values
+        for v_ego in v_ego_arr:
+            expected = float(np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))
+            actual = float(np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V))
+            np.testing.assert_equal(actual, expected)
 
-    for v_ego in v_ego_arr:
-      expected = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-      actual = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
-      np.testing.assert_equal(actual, expected)

scripts/post-commit

@@ -1,7 +1,3 @@
-#!/usr/bin/env bash
-set -e
-if [[ -f .git/hooks/post-commit.d/post-commit ]]; then
-  .git/hooks/post-commit.d/post-commit
-fi
-tools/op.sh lint --fast
-echo ""
+#!/bin/sh
+command -v git-lfs >/dev/null 2>&1 || { echo >&2 "\nThis repository is configured for Git LFS but 'git-lfs' was not found on your path. If you no longer wish to use Git LFS, remove this hook by deleting the 'post-commit' file in the hooks directory (set by 'core.hookspath'; usually '.git/hooks').\n"; exit 2; }
+git lfs post-commit "$@"

selfdrive/car/cruise.py

@@ -1,8 +1,7 @@
 import math
-
+import numpy as np
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
-from openpilot.common.numpy_fast import clip
 
 
 # WARNING: this value was determined based on the model's training distribution,
@@ -106,7 +105,7 @@ def _update_v_cruise_non_pcm(self, CS, enabled, is_metric):
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
       self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
 
-    self.v_cruise_kph = clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
+    self.v_cruise_kph = float(np.clip(round(self.v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))
 
   def update_button_timers(self, CS, enabled):
     # increment timer for buttons still pressed
@@ -130,6 +129,6 @@ def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last
     else:
-      self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
+      self.v_cruise_kph = int(round(float(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))))
 
     self.v_cruise_cluster_kph = self.v_cruise_kph

selfdrive/car/tests/test_cruise_speed.py

@@ -125,7 +125,7 @@ def test_set_gas_pressed(self):
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
       expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH)  # clip to min of vEgo
-      expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))
+      expected_v_cruise_kph = np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
 
       CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True})
       CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]

selfdrive/controls/controlsd.py

@@ -106,15 +106,17 @@ def state_control(self):
 
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
-    actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)
+    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits))
 
     # Steering PID loop and lateral MPC
     self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
-    actuators.curvature = self.desired_curvature
-    actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+    actuators.curvature = float(self.desired_curvature)
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                             self.steer_limited, self.desired_curvature,
                                                                             self.calibrated_pose) # TODO what if not available
-
+    actuators.steer = float(steer)
+    actuators.steeringAngleDeg = float(steeringAngleDeg)
+    actuators.steer = float(steer)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)
@@ -179,7 +181,7 @@ def publish(self, CC, lac_log):
 
     cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
     cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
-    cs.desiredCurvature = self.desired_curvature
+    cs.desiredCurvature = float(self.desired_curvature)
     cs.longControlState = self.LoC.long_control_state
     cs.upAccelCmd = float(self.LoC.pid.p)
     cs.uiAccelCmd = float(self.LoC.pid.i)

selfdrive/controls/lib/drive_helpers.py

@@ -1,5 +1,5 @@
+import numpy as np
 from cereal import log
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 
 MIN_SPEED = 1.0
@@ -13,19 +13,19 @@
 MAX_VEL_ERR = 5.0
 
 def clip_curvature(v_ego, prev_curvature, new_curvature):
-  new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
+  new_curvature = float(np.clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE))
   v_ego = max(MIN_SPEED, v_ego)
   max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
-  safe_desired_curvature = clip(new_curvature,
+  safe_desired_curvature = float(np.clip(new_curvature,
                                 prev_curvature - max_curvature_rate * DT_CTRL,
-                                prev_curvature + max_curvature_rate * DT_CTRL)
+                                prev_curvature + max_curvature_rate * DT_CTRL))
 
   return safe_desired_curvature
 
 
 def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
   # ToDo: Try relative error, and absolute speed
   if len(modelV2.temporalPose.trans):
-    vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
+    vel_err = float(np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR))
     return float(vel_err)
   return 0.0

selfdrive/controls/lib/latcontrol.py

@@ -1,6 +1,6 @@
 from abc import abstractmethod, ABC
 
-from openpilot.common.numpy_fast import clip
+import numpy as np
 from openpilot.common.realtime import DT_CTRL
 
 MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
@@ -28,5 +28,5 @@ def _check_saturation(self, saturated, CS, steer_limited):
       self.sat_count += self.sat_count_rate
     else:
       self.sat_count -= self.sat_count_rate
-    self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
+    self.sat_count = float(np.clip(self.sat_count, 0.0, self.sat_limit))
     return self.sat_count > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_angle.py

@@ -23,7 +23,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
       angle_steers_des += params.angleOffsetDeg
 
     angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
-    angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False)
+    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False))
     angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     angle_log.steeringAngleDesiredDeg = angle_steers_des
     return 0, float(angle_steers_des), angle_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -39,10 +39,10 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
       output_steer = self.pid.update(error, override=CS.steeringPressed,
                                      feedforward=steer_feedforward, speed=CS.vEgo)
       pid_log.active = True
-      pid_log.p = self.pid.p
-      pid_log.i = self.pid.i
-      pid_log.f = self.pid.f
-      pid_log.output = output_steer
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(output_steer)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited))
 
     return output_steer, angle_steers_des, pid_log

selfdrive/controls/lib/latcontrol_torque.py

@@ -2,7 +2,7 @@
 
 from cereal import log
 from opendbc.car.interfaces import LatControlInputs
-from openpilot.common.numpy_fast import interp
+import numpy as np
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@@ -51,7 +51,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
       else:
         assert calibrated_pose is not None
         actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo
-        actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
+        actual_curvature = float(np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]))
         curvature_deadzone = 0.0
       desired_lateral_accel = desired_curvature * CS.vEgo ** 2
 
@@ -60,7 +60,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, calib
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
-      low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
+      low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y))**2
       setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
       measurement = actual_lateral_accel + low_speed_factor * actual_curvature
       gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation

selfdrive/controls/lib/longcontrol.py

@@ -1,5 +1,5 @@
 from cereal import car
-from openpilot.common.numpy_fast import clip
+import numpy as np
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
@@ -84,5 +84,5 @@ def update(self, active, CS, a_target, should_stop, accel_limits):
       output_accel = self.pid.update(error, speed=CS.vEgo,
                                      feedforward=a_target)
 
-    self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
+    self.last_output_accel = float(np.clip(output_accel, accel_limits[0], accel_limits[1]))
     return self.last_output_accel

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -4,7 +4,6 @@
 import numpy as np
 from cereal import log
 from opendbc.car.interfaces import ACCEL_MIN
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
@@ -320,9 +319,9 @@ def process_lead(self, lead):
     # MPC will not converge if immediate crash is expected
     # Clip lead distance to what is still possible to brake for
     min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
-    x_lead = clip(x_lead, min_x_lead, 1e8)
-    v_lead = clip(v_lead, 0.0, 1e8)
-    a_lead = clip(a_lead, -10., 5.)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
+    a_lead = np.clip(a_lead, -10., 5.)
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv