diff --git a/common/pid.py b/common/pid.py index 36cbf9c4e9..f2ab935f45 100644 --- a/common/pid.py +++ b/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) - self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) + control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit) + 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 diff --git a/common/tests/test_numpy_fast.py b/common/tests/test_numpy_fast.py deleted file mode 100644 index aa53851db0..0000000000 --- a/common/tests/test_numpy_fast.py +++ /dev/null @@ -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) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index b92d0c7465..697d0273a5 100644 --- a/selfdrive/car/cruise.py +++ b/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 64be434081..c215a42a6d 100755 --- a/selfdrive/controls/controlsd.py +++ b/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.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) # 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) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 66b92319ca..dcb0093a18 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 0e4a27da61..f84f70be4f 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/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) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index d3da656daa..4162bd62dc 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index d7edd39f8a..dedc97a964 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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.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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 55a37c1f4b..d66e5be28d 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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.i = self.pid.i - pid_log.d = self.pid.d - pid_log.f = self.pid.f - pid_log.output = -output_torque - pid_log.actualLateralAccel = actual_lateral_accel - pid_log.desiredLateralAccel = desired_lateral_accel - pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) + pid_log.p = float(self.pid.p) + pid_log.i = float(self.pid.i) + pid_log.d = float(self.pid.d) + pid_log.f = float(self.pid.f) + pid_log.output = float(-output_torque) + pid_log.actualLateralAccel = float(actual_lateral_accel) + pid_log.desiredLateralAccel = float(desired_lateral_accel) + 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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e18ee0c279..6d4f922461 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 65e1421d77..579ee85fd4 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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) - 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117..5560dfbb09 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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.shouldStop = should_stop + longitudinalPlan.aTarget = float(a_target) + longitudinalPlan.shouldStop = bool(should_stop) longitudinalPlan.allowBrake = True - longitudinalPlan.allowThrottle = self.allow_throttle + longitudinalPlan.allowThrottle = bool(self.allow_throttle) pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index fb69fb736f..ea05c00d35 100755 --- a/selfdrive/controls/radard.py +++ b/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: diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 8549b92665..ee0524ec9d 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/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) diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 19ded3b4f7..da2f3f20bb 100755 --- a/selfdrive/locationd/paramsd.py +++ b/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.angleOffsetAverageDeg = angle_offset_average - liveParameters.angleOffsetDeg = angle_offset + liveParameters.roll = float(roll) + liveParameters.angleOffsetAverageDeg = float(angle_offset_average) + liveParameters.angleOffsetDeg = float(angle_offset) liveParameters.valid = all(( avg_offset_valid, total_offset_valid, diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 88665778a2..337be3a2ae 100644 --- a/selfdrive/monitoring/helpers.py +++ b/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 diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index c08ac6d369..026c8ce22a 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/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.] diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index f72e183cde..be93d5922a 100755 --- a/system/hardware/fan_controller.py +++ b/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 diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index d31546e058..60131ba370 100755 --- a/tools/joystick/joystick_control.py +++ b/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: diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 3e3aed34a9..21d10b36d4 100755 --- a/tools/joystick/joystickd.py +++ b/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) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index bbbfd9a981..bf3be5193d 100644 --- a/tools/sim/bridge/common.py +++ b/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) - brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0) + throttle_op = np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 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