Getting rid of openpilot.common.numpy_fast (#34368)

* Got rid openpilot.common.numpy_fast

* fixed some data type erros

* importing numpy instead of importing specific functions

* fixing some numpy importing mistakes

* Update selfdrive/car/cruise.py

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

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 @@ class PIDController:
 
   @property
   def k_p(self):
-    return interp(self.speed, self._k_p[0], self._k_p[1])
+    return 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 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 np.interp(self.speed, self._k_d[0], self._k_d[1])
 
   @property
   def error_integral(self):
@@ -64,10 +61,10 @@ class PIDController:
 
         # 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)
+        control_no_i = np.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)
+        self.i = 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 = np.clip(control, self.neg_limit, self.pos_limit)
     return self.control

common/tests/test_numpy_fast.py

@@ -1,21 +0,0 @@
-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]
-
-    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)
-
-    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)

selfdrive/car/cruise.py

@@ -1,8 +1,8 @@
 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 +106,7 @@ class VCruiseHelper:
     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 = 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 +130,6 @@ class VCruiseHelper:
     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(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
 
     self.v_cruise_cluster_kph = self.v_cruise_kph

selfdrive/controls/controlsd.py

@@ -106,15 +106,16 @@ class Controls:
 
     # 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.curvature = float(self.desired_curvature)
-    actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+    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)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)
@@ -179,7 +180,7 @@ class Controls:
 
     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,10 +13,10 @@ MAX_LATERAL_JERK = 5.0
 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 = 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 = np.clip(new_curvature,
                                 prev_curvature - max_curvature_rate * DT_CTRL,
                                 prev_curvature + max_curvature_rate * DT_CTRL)
 
@@ -26,6 +26,6 @@ def clip_curvature(v_ego, prev_curvature, new_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 = 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 @@
+import numpy as np
 from abc import abstractmethod, ABC
 
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 
 MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
@@ -28,5 +28,5 @@ class LatControl(ABC):
       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 = 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 @@ class LatControlAngle(LatControl):
       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 @@ class LatControlPID(LatControl):
       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.p = float(self.pid.p)
-      pid_log.i = self.pid.i
+      pid_log.i = float(self.pid.i)
-      pid_log.f = self.pid.f
+      pid_log.f = float(self.pid.f)
-      pid_log.output = output_steer
+      pid_log.output = float(output_steer)
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+      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

@@ -1,8 +1,8 @@
 import math
+import numpy as np
 
 from cereal import log
 from opendbc.car.interfaces import LatControlInputs
-from openpilot.common.numpy_fast import interp
 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 @@ class LatControlTorque(LatControl):
       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 = 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 @@ class LatControlTorque(LatControl):
       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 = 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
@@ -68,7 +68,7 @@ class LatControlTorque(LatControl):
                                                             setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
       torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
                                                                measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
-      pid_log.error = torque_from_setpoint - torque_from_measurement
+      pid_log.error = float(torque_from_setpoint - torque_from_measurement)
       ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
                                           desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
                                           gravity_adjusted=True)
@@ -80,14 +80,14 @@ class LatControlTorque(LatControl):
                                       freeze_integrator=freeze_integrator)
 
       pid_log.active = True
-      pid_log.p = self.pid.p
+      pid_log.p = float(self.pid.p)
-      pid_log.i = self.pid.i
+      pid_log.i = float(self.pid.i)
-      pid_log.d = self.pid.d
+      pid_log.d = float(self.pid.d)
-      pid_log.f = self.pid.f
+      pid_log.f = float(self.pid.f)
-      pid_log.output = -output_torque
+      pid_log.output = float(-output_torque)
-      pid_log.actualLateralAccel = actual_lateral_accel
+      pid_log.actualLateralAccel = float(actual_lateral_accel)
-      pid_log.desiredLateralAccel = desired_lateral_accel
+      pid_log.desiredLateralAccel = float(desired_lateral_accel)
-      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited))
 
     # TODO left is positive in this convention
     return -output_torque, 0.0, pid_log

selfdrive/controls/lib/longcontrol.py

@@ -1,5 +1,5 @@
+import numpy as np
 from cereal import car
-from openpilot.common.numpy_fast import clip
 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 @@ class LongControl:
       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 = 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 time
 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 @@ class LongitudinalMpc:
     # 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)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
-    v_lead = clip(v_lead, 0.0, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
-    a_lead = clip(a_lead, -10., 5.)
+    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
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from openpilot.common.numpy_fast import clip, interp
 
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
@@ -30,7 +29,7 @@ _A_TOTAL_MAX_BP = [20., 40.]
 
 
 def get_max_accel(v_ego):
-  return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
@@ -43,7 +42,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
   # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
   # The lookup table for turns should also be updated if we do this
-  a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
   a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
   a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
 
@@ -55,9 +54,9 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
     v_now = speeds[0]
     a_now = accels[0]
 
-    v_target = interp(action_t, CONTROL_N_T_IDX, speeds)
+    v_target = np.interp(action_t, CONTROL_N_T_IDX, speeds)
     a_target = 2 * (v_target - v_now) / (action_t) - a_now
-    v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
+    v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
   else:
     v_target = 0.0
     v_target_1sec = 0.0
@@ -139,7 +138,7 @@ class LongitudinalPlanner:
     if reset_state:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active
-      self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
+      self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -151,7 +150,7 @@ class LongitudinalPlanner:
 
     if not self.allow_throttle:
       clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
-      clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
+      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
       accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
 
     if force_slow_decel:
@@ -176,7 +175,7 @@ class LongitudinalPlanner:
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
-    self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
+    self.a_desired = float(np.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, sm, pm):
@@ -200,9 +199,9 @@ class LongitudinalPlanner:
     action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
     a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
                                                 action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
-    longitudinalPlan.aTarget = a_target
+    longitudinalPlan.aTarget = float(a_target)
-    longitudinalPlan.shouldStop = should_stop
+    longitudinalPlan.shouldStop = bool(should_stop)
     longitudinalPlan.allowBrake = True
-    longitudinalPlan.allowThrottle = self.allow_throttle
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)

selfdrive/controls/radard.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python3
 import math
+import numpy as np
 from collections import deque
 from typing import Any
 
 import capnp
 from cereal import messaging, log, car
-from openpilot.common.numpy_fast import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
 from openpilot.common.swaglog import cloudlog
@@ -44,7 +44,7 @@ class KalmanParams:
           0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
           0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
           0.26393339, 0.26278425]
-    self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
+    self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
 
 
 class Track:

selfdrive/debug/live_cpu_and_temp.py

@@ -1,10 +1,10 @@
 #!/usr/bin/env python3
 import argparse
+import numpy as np
 import capnp
 from collections import defaultdict
 
 from cereal.messaging import SubMaster
-from openpilot.common.numpy_fast import mean
 
 def cputime_total(ct):
   return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
@@ -49,7 +49,7 @@ if __name__ == "__main__":
 
     if sm.updated['deviceState']:
       t = sm['deviceState']
-      last_temp = mean(t.cpuTempC)
+      last_temp = np.mean(t.cpuTempC)
       last_mem = t.memoryUsagePercent
 
     if sm.updated['procLog']:
@@ -72,7 +72,7 @@ if __name__ == "__main__":
       total_times = total_times_new[:]
       busy_times = busy_times_new[:]
 
-      print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
+      print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
 
       if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
         procs: dict[str, float] = defaultdict(float)

selfdrive/locationd/paramsd.py

@@ -8,7 +8,6 @@ import cereal.messaging as messaging
 from cereal import car, log
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
-from openpilot.common.numpy_fast import clip
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
@@ -66,7 +65,7 @@ class ParamsLearner:
         # This is done to bound the road roll estimate when localizer values are invalid
         roll = 0.0
         roll_std = np.radians(10.0)
-      self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+      self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
 
       yaw_rate_valid = msg.angularVelocityDevice.valid and self.calibrator.calib_valid
       yaw_rate_valid = yaw_rate_valid and 0 < self.yaw_rate_std < 10  # rad/s
@@ -203,11 +202,11 @@ def main():
         learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
         x = learner.kf.x
 
-      angle_offset_average = clip(math.degrees(x[States.ANGLE_OFFSET].item()),
+      angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
                                   angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
-      angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+      angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
                           angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
-      roll = clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
+      roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
       roll_std = float(P[States.ROAD_ROLL].item())
       if learner.active and learner.speed > LOW_ACTIVE_SPEED:
         # Account for the opposite signs of the yaw rates
@@ -226,9 +225,9 @@ def main():
       liveParameters.sensorValid = sensors_valid
       liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
       liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-      liveParameters.roll = roll
+      liveParameters.roll = float(roll)
-      liveParameters.angleOffsetAverageDeg = angle_offset_average
+      liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
-      liveParameters.angleOffsetDeg = angle_offset
+      liveParameters.angleOffsetDeg = float(angle_offset)
       liveParameters.valid = all((
         avg_offset_valid,
         total_offset_valid,

selfdrive/monitoring/helpers.py

@@ -1,9 +1,9 @@
 from math import atan2
+import numpy as np
 
 from cereal import car, log
 import cereal.messaging as messaging
 from openpilot.selfdrive.selfdrived.events import Events
-from openpilot.common.numpy_fast import interp
 from openpilot.common.realtime import DT_DMON
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.stat_live import RunningStatFilter
@@ -205,10 +205,10 @@ class DriverMonitoring:
     bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
     k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
     bp_normal = max(min(bp / k1, 0.5),0)
-    self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5],
+    self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5],
                                            [self.settings._POSE_PITCH_THRESHOLD_SLACK,
                                             self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
-    self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5],
+    self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5],
                                            [self.settings._POSE_YAW_THRESHOLD_SLACK,
                                             self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
 

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -121,7 +121,7 @@ class Plant:
     ss.selfdriveState.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
-    car_state.carState.standstill = self.speed < 0.01
+    car_state.carState.standstill = bool(self.speed < 0.01)
     car_state.carState.vCruise = float(v_cruise * 3.6)
     car_control.carControl.orientationNED = [0., float(pitch), 0.]
 

system/hardware/fan_controller.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
+import numpy as np
 from abc import ABC, abstractmethod
 
 from openpilot.common.realtime import DT_HW
-from openpilot.common.numpy_fast import interp
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.pid import PIDController
 
@@ -30,7 +30,7 @@ class TiciFanController(BaseFanController):
     error = 70 - cur_temp
     fan_pwr_out = -int(self.controller.update(
                       error=error,
-                      feedforward=interp(cur_temp, [60.0, 100.0], [0, -100])
+                      feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100])
                     ))
 
     self.last_ignition = ignition

tools/joystick/joystick_control.py

@@ -2,10 +2,10 @@
 import os
 import argparse
 import threading
+import numpy as np
 from inputs import UnpluggedError, get_gamepad
 
 from cereal import messaging
-from openpilot.common.numpy_fast import interp, clip
 from openpilot.common.params import Params
 from openpilot.common.realtime import Ratekeeper
 from openpilot.system.hardware import HARDWARE
@@ -34,7 +34,7 @@ class Keyboard:
     elif key in self.axes_map:
       axis = self.axes_map[key]
       incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment
-      self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1)
+      self.axes_values[axis] = np.clip(self.axes_values[axis] + incr, -1, 1)
     else:
       return False
     return True
@@ -83,7 +83,7 @@ class Joystick:
       self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]])
       self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]])
 
-      norm = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
+      norm = -np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.])
       norm = norm if abs(norm) > 0.03 else 0.  # center can be noisy, deadzone of 3%
       self.axes_values[event[0]] = EXPO * norm ** 3 + (1 - EXPO) * norm  # less action near center for fine control
     else:

tools/joystick/joystickd.py

@@ -1,9 +1,9 @@
 #!/usr/bin/env python3
 
 import math
+import numpy as np
 
 from cereal import messaging, car
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL, Ratekeeper
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
@@ -45,13 +45,13 @@ def joystickd_thread():
       joystick_axes = [0.0, 0.0]
 
     if CC.longActive:
-      actuators.accel = 4.0 * clip(joystick_axes[0], -1, 1)
+      actuators.accel = 4.0 * np.clip(joystick_axes[0], -1, 1)
 
     if CC.latActive:
       max_curvature = MAX_LAT_ACCEL / max(sm['carState'].vEgo ** 2, 5)
       max_angle = math.degrees(VM.get_steer_from_curvature(max_curvature, sm['carState'].vEgo, sm['liveParameters'].roll))
 
-      actuators.steer = clip(joystick_axes[1], -1, 1)
+      actuators.steer = np.clip(joystick_axes[1], -1, 1)
       actuators.steeringAngleDeg, actuators.curvature = actuators.steer * max_angle, actuators.steer * -max_curvature
 
     pm.send('carControl', cc_msg)

tools/sim/bridge/common.py

@@ -1,6 +1,7 @@
 import signal
 import threading
 import functools
+import numpy as np
 
 from collections import namedtuple
 from enum import Enum
@@ -9,7 +10,6 @@ from abc import ABC, abstractmethod
 
 from opendbc.car.honda.values import CruiseButtons
 from openpilot.common.params import Params
-from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import Ratekeeper
 from openpilot.selfdrive.test.helpers import set_params_enabled
 from openpilot.tools.sim.lib.common import SimulatorState, World
@@ -173,8 +173,8 @@ Ignition: {self.simulator_state.ignition} Engaged: {self.simulator_state.is_enga
       self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active
 
       if self.simulator_state.is_engaged:
-        throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
+        throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)
-        brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
+        brake_op = np.clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0)
         steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg
 
         self.past_startup_engaged = True