From 750564390087babb893f8808c6bd16f276ec22c9 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Sat, 11 Jan 2025 06:11:36 +0000 Subject: [PATCH 01/13] Removed numpy_fast and updated dependent files to eliminate dependency --- common/numpy_fast.py | 19 ------------------- common/pid.py | 15 ++++++--------- common/tests/test_numpy_fast.py | 5 ++--- scripts/post-commit | 10 +++------- selfdrive/car/cruise.py | 6 +++--- selfdrive/controls/lib/drive_helpers.py | 8 ++++---- selfdrive/controls/lib/latcontrol.py | 4 ++-- selfdrive/controls/lib/latcontrol_torque.py | 6 +++--- selfdrive/controls/lib/longcontrol.py | 4 ++-- .../lib/longitudinal_mpc_lib/long_mpc.py | 7 +++---- .../controls/lib/longitudinal_planner.py | 16 ++++++++-------- selfdrive/controls/radard.py | 4 ++-- selfdrive/debug/live_cpu_and_temp.py | 6 +++--- selfdrive/locationd/paramsd.py | 9 ++++----- selfdrive/monitoring/helpers.py | 6 +++--- system/hardware/fan_controller.py | 4 ++-- tools/joystick/joystick_control.py | 6 +++--- tools/joystick/joystickd.py | 6 +++--- tools/sim/bridge/common.py | 6 +++--- 19 files changed, 59 insertions(+), 88 deletions(-) delete mode 100644 common/numpy_fast.py diff --git a/common/numpy_fast.py b/common/numpy_fast.py deleted file mode 100644 index 878c0005c8e811..00000000000000 --- a/common/numpy_fast.py +++ /dev/null @@ -1,19 +0,0 @@ -def clip(x, lo, hi): - return max(lo, min(hi, x)) - -def interp(x, xp, fp): - N = len(xp) - - def get_interp(xv): - hi = 0 - while hi < N and xv > xp[hi]: - hi += 1 - low = hi - 1 - return fp[-1] if hi == N and xv > xp[low] else ( - fp[0] if hi == 0 else - (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) - - return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x) - -def mean(x): - return sum(x) / len(x) diff --git a/common/pid.py b/common/pid.py index 36cbf9c4e9b59e..f2ab935f4584f6 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 @@ 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 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 @@ 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 = 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 index aa53851db04981..e7b8227c36714f 100644 --- a/common/tests/test_numpy_fast.py +++ b/common/tests/test_numpy_fast.py @@ -1,6 +1,5 @@ import numpy as np -from openpilot.common.numpy_fast import interp class TestInterp: @@ -11,11 +10,11 @@ def test_correctness_controls(self): 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) + actual = np.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) + actual = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) np.testing.assert_equal(actual, expected) diff --git a/scripts/post-commit b/scripts/post-commit index f9964639de4137..52b339cb3f4966 100755 --- a/scripts/post-commit +++ b/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 "$@" diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index b92d0c74659db3..ed335ba83ca8de 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -2,7 +2,7 @@ from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import clip +import numpy as np # WARNING: this value was determined based on the model's training distribution, @@ -106,7 +106,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 = 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 @@ 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(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/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 66b92319ca03cc..58b629fead20c1 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,5 @@ from cereal import log -from openpilot.common.numpy_fast import clip +import numpy np from openpilot.common.realtime import DT_CTRL MIN_SPEED = 1.0 @@ -13,10 +13,10 @@ 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 0e4a27da61725f..aa16516afbfbaa 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,6 +1,6 @@ from abc import abstractmethod, ABC -from openpilot.common.numpy_fast import clip +import numpy 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 = 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_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 55a37c1f4bd5ad..631335f6a60c6f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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 = 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 = 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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e18ee0c279024b..f4be550e84b6b8 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/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 = 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 65e1421d779247..579ee85fd44375 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 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117166b..f340f1c3e405b1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,7 +1,7 @@ #!/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 +30,7 @@ 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 +43,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 +55,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 +139,7 @@ def update(self, sm): 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 +151,7 @@ def update(self, sm): 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 +176,7 @@ def update(self, sm): # 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): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index fb69fb736fdedc..55846ecbfba9b4 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -5,7 +5,7 @@ import capnp from cereal import messaging, log, car -from openpilot.common.numpy_fast import interp +import numpy as np 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 @@ def __init__(self, dt: float): 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 8549b92665e6a1..29249e98ff8613 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -4,7 +4,7 @@ from collections import defaultdict from cereal.messaging import SubMaster -from openpilot.common.numpy_fast import mean +import numpy as np def cputime_total(ct): return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq @@ -49,7 +49,7 @@ def proc_name(proc): 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 @@ def proc_name(proc): 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 19ded3b4f7176b..7c81554a62f348 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -8,7 +8,6 @@ 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 @@ def handle_log(self, t, which, msg): # 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 diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 88665778a2b7a9..07c4b5c5bed0f3 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -3,7 +3,7 @@ from cereal import car, log import cereal.messaging as messaging from openpilot.selfdrive.selfdrived.events import Events -from openpilot.common.numpy_fast import interp +import numpy as np 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 @@ def _set_policy(self, model_data, car_speed): 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/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index f72e183cde9479..f88b77254d1d46 100755 --- a/system/hardware/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -2,7 +2,7 @@ from abc import ABC, abstractmethod from openpilot.common.realtime import DT_HW -from openpilot.common.numpy_fast import interp +import numpy as np from openpilot.common.swaglog import cloudlog from openpilot.common.pid import PIDController @@ -30,7 +30,7 @@ def update(self, cur_temp: float, ignition: bool) -> int: 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 d31546e058a2ac..a854014cb02a41 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -5,7 +5,7 @@ from inputs import UnpluggedError, get_gamepad from cereal import messaging -from openpilot.common.numpy_fast import interp, clip +import numpy np from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.system.hardware import HARDWARE @@ -34,7 +34,7 @@ def update(self): 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 @@ def update(self): 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 3e3aed34a94791..3041c005fa37e0 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -3,7 +3,7 @@ import math from cereal import messaging, car -from openpilot.common.numpy_fast import clip +import numpy as np 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 bbbfd9a98182c5..d934730a874ff6 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -9,7 +9,7 @@ from opendbc.car.honda.values import CruiseButtons from openpilot.common.params import Params -from openpilot.common.numpy_fast import clip +import numpy as np 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 @@ def _run(self, q: Queue): 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 From ffb174ea302df85a51b0db0afab0f7a77385bef2 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Sat, 11 Jan 2025 11:20:13 +0000 Subject: [PATCH 02/13] removed numpy_fast and updated the dependent files --- selfdrive/controls/lib/drive_helpers.py | 2 +- selfdrive/controls/lib/latcontrol.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 58b629fead20c1..7f29299ac0bb2a 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,5 @@ from cereal import log -import numpy np +import numpy as np from openpilot.common.realtime import DT_CTRL MIN_SPEED = 1.0 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index aa16516afbfbaa..48f791c26b90b3 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -1,6 +1,6 @@ from abc import abstractmethod, ABC -import numpy np +import numpy as np from openpilot.common.realtime import DT_CTRL MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s From 55a45cee94ea96d2a850e71ea95c651ca1ab8cc6 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Sun, 12 Jan 2025 05:18:40 +0000 Subject: [PATCH 03/13] numpy_fast issue --- selfdrive/controls/lib/latcontrol_torque.py | 2 +- selfdrive/debug/live_cpu_and_temp.py | 2 +- selfdrive/monitoring/helpers.py | 2 +- tools/joystick/joystick_control.py | 2 +- tools/joystick/joystickd.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 631335f6a60c6f..32ab75408861fe 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -2,7 +2,7 @@ from cereal import log from opendbc.car.interfaces import LatControlInputs -import numpy as np +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 diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 29249e98ff8613..f9142299c994af 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -4,7 +4,7 @@ from collections import defaultdict from cereal.messaging import SubMaster -import numpy as np +import numpy as np def cputime_total(ct): return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index 07c4b5c5bed0f3..f22dea38e0c9dc 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -3,7 +3,7 @@ from cereal import car, log import cereal.messaging as messaging from openpilot.selfdrive.selfdrived.events import Events -import numpy as np +import numpy as np from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter diff --git a/tools/joystick/joystick_control.py b/tools/joystick/joystick_control.py index a854014cb02a41..375e429758c3de 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -5,7 +5,7 @@ from inputs import UnpluggedError, get_gamepad from cereal import messaging -import numpy np +import numpy as np from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper from openpilot.system.hardware import HARDWARE diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 3041c005fa37e0..60e85030bc488b 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -3,7 +3,7 @@ import math from cereal import messaging, car -import numpy as np +import numpy as np from openpilot.common.realtime import DT_CTRL, Ratekeeper from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog From b33c65129f4949c3a507b6de658f9ed964f48cfb Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Sun, 12 Jan 2025 07:17:18 +0000 Subject: [PATCH 04/13] patch of pycap --- common/pid.py | 12 +++++----- common/tests/test_numpy_fast.py | 8 +++---- selfdrive/car/cruise.py | 4 ++-- selfdrive/controls/lib/drive_helpers.py | 8 +++---- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 4 ++-- selfdrive/controls/lib/longcontrol.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 14 +++++------ .../controls/lib/longitudinal_planner.py | 24 +++++++++---------- selfdrive/controls/radard.py | 3 ++- selfdrive/debug/live_cpu_and_temp.py | 4 ++-- selfdrive/locationd/paramsd.py | 12 +++++----- selfdrive/monitoring/helpers.py | 8 +++---- system/hardware/fan_controller.py | 2 +- tools/joystick/joystick_control.py | 4 ++-- tools/joystick/joystickd.py | 4 ++-- tools/sim/bridge/common.py | 4 ++-- 17 files changed, 60 insertions(+), 59 deletions(-) diff --git a/common/pid.py b/common/pid.py index f2ab935f4584f6..b0f7299702f3d6 100644 --- a/common/pid.py +++ b/common/pid.py @@ -25,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 np.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 np.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 np.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): @@ -61,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 = 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_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 = np.clip(control, self.neg_limit, self.pos_limit) + self.control = float(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 index e7b8227c36714f..ec2f5ea099b135 100644 --- a/common/tests/test_numpy_fast.py +++ b/common/tests/test_numpy_fast.py @@ -9,12 +9,12 @@ def test_correctness_controls(self): 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 = np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + expected = float(np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)) + actual = float(np.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 = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) + 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) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index ed335ba83ca8de..aba4cc7fcdc5a4 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -106,7 +106,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 = np.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 +130,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(np.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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 7f29299ac0bb2a..9ee2da5727b7f9 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -13,12 +13,12 @@ MAX_VEL_ERR = 5.0 def clip_curvature(v_ego, prev_curvature, new_curvature): - new_curvature = np.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 = np.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 @@ -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 = np.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 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 48f791c26b90b3..1a5c012c0933cd 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 = np.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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 32ab75408861fe..cb43e96ad26cbf 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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 = np.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 = np.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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f4be550e84b6b8..99ebdbbb582a3b 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -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 = np.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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 579ee85fd44375..517e3e1b04fe9f 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -297,7 +297,7 @@ def set_cur_state(self, v, a): @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) - v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) + v_lead_traj = float(np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)) x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv @@ -319,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 = 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.) + x_lead = float(np.clip(x_lead, min_x_lead, 1e8)) + v_lead = float(np.clip(v_lead, 0.0, 1e8)) + a_lead = float(np.clip(a_lead, -10., 5.)) lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv @@ -358,9 +358,9 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), + v_cruise_clipped = float(np.clip(v_cruise * np.ones(N+1), v_lower, - v_upper) + v_upper)) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -373,7 +373,7 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) - cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] + cruise_target = T_IDXS * float(np.clip(v_cruise, v_ego - 2.0, 1e3)) + x[0] xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) x = np.cumsum(np.insert(xforward, 0, x[0])) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f340f1c3e405b1..b9ff21d73347a3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -30,7 +30,7 @@ def get_max_accel(v_ego): - return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) + return float(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 +43,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 = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_total_max = float(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 +55,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 = np.interp(action_t, CONTROL_N_T_IDX, speeds) + v_target = float(np.interp(action_t, CONTROL_N_T_IDX, speeds)) a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds) + v_target_1sec = float(np.interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)) else: v_target = 0.0 v_target_1sec = 0.0 @@ -89,9 +89,9 @@ def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and len(model_msg.velocity.x) == ModelConstants.IDX_N and len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) + x = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)) - model_error * T_IDXS_MPC + v = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)) - model_error + a = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)) j = np.zeros(len(T_IDXS_MPC)) else: x = np.zeros(len(T_IDXS_MPC)) @@ -139,7 +139,7 @@ def update(self, sm): 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 = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) + self.a_desired = float(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 +151,7 @@ def update(self, sm): if not self.allow_throttle: clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) + clipped_accel_coast_interp = float(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: @@ -165,9 +165,9 @@ def update(self, sm): self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) - self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) - self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) - self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) + self.v_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)) + self.a_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)) + self.j_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)) # TODO counter is only needed because radar is glitchy, remove once radar is gone self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 55846ecbfba9b4..1507259f34a503 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -44,7 +44,8 @@ def __init__(self, dt: float): 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 = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]] + self.K = [[float(np.interp(dt, dts, K0))], [float(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 f9142299c994af..906d77c2385f5d 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -49,7 +49,7 @@ def proc_name(proc): if sm.updated['deviceState']: t = sm['deviceState'] - last_temp = np.mean(t.cpuTempC) + last_temp = float(np.mean(t.cpuTempC)) last_mem = t.memoryUsagePercent if sm.updated['procLog']: @@ -72,7 +72,7 @@ def proc_name(proc): total_times = total_times_new[:] busy_times = busy_times_new[:] - print(f"CPU {100.0 * np.mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C") + print(f"CPU {100.0 * float(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 7c81554a62f348..15391bc1e9815a 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -65,7 +65,7 @@ def handle_log(self, t, which, msg): # 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 = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + self.roll = float(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 @@ -202,11 +202,11 @@ def main(): learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) x = learner.kf.x - 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 = 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 = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA) + angle_offset_average = float(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 = float(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 = float(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 diff --git a/selfdrive/monitoring/helpers.py b/selfdrive/monitoring/helpers.py index f22dea38e0c9dc..6cc32c6b90895f 100644 --- a/selfdrive/monitoring/helpers.py +++ b/selfdrive/monitoring/helpers.py @@ -205,12 +205,12 @@ def _set_policy(self, model_data, car_speed): 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 = np.interp(bp_normal, [0, 0.5], + self.pose.cfactor_pitch = float(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 = np.interp(bp_normal, [0, 0.5], + self.settings._POSE_PITCH_THRESHOLD_STRICT])) / self.settings._POSE_PITCH_THRESHOLD + self.pose.cfactor_yaw = float(np.interp(bp_normal, [0, 0.5], [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD + self.settings._POSE_YAW_THRESHOLD_STRICT])) / self.settings._POSE_YAW_THRESHOLD def _get_distracted_types(self): distracted_types = [] diff --git a/system/hardware/fan_controller.py b/system/hardware/fan_controller.py index f88b77254d1d46..56c850227c144f 100755 --- a/system/hardware/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -30,7 +30,7 @@ def update(self, cur_temp: float, ignition: bool) -> int: error = 70 - cur_temp fan_pwr_out = -int(self.controller.update( error=error, - feedforward=np.interp(cur_temp, [60.0, 100.0], [0, -100]) + feedforward=float(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 375e429758c3de..3914166f785e69 100755 --- a/tools/joystick/joystick_control.py +++ b/tools/joystick/joystick_control.py @@ -34,7 +34,7 @@ def update(self): 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] = np.clip(self.axes_values[axis] + incr, -1, 1) + self.axes_values[axis] = float(np.clip(self.axes_values[axis] + incr, -1, 1)) else: return False return True @@ -83,7 +83,7 @@ def update(self): 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 = -np.interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]) + norm = float(-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 60e85030bc488b..b48c44e2ddb133 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -45,13 +45,13 @@ def joystickd_thread(): joystick_axes = [0.0, 0.0] if CC.longActive: - actuators.accel = 4.0 * np.clip(joystick_axes[0], -1, 1) + actuators.accel = 4.0 * float(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 = np.clip(joystick_axes[1], -1, 1) + actuators.steer = float(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 d934730a874ff6..f982f66f307e40 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -173,8 +173,8 @@ def _run(self, q: Queue): self.simulator_state.is_engaged = self.simulated_car.sm['selfdriveState'].active if self.simulator_state.is_engaged: - 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) + throttle_op = float(np.clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0)) + brake_op = float(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 From beffe069c0e5a9506b0efa0647bc81539cc0bd40 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 02:22:16 +0000 Subject: [PATCH 05/13] float comversion --- .../controls/lib/longitudinal_planner.py | 24 +++++++++---------- selfdrive/debug/live_cpu_and_temp.py | 4 ++-- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b9ff21d73347a3..31a467f23c693c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -30,7 +30,7 @@ def get_max_accel(v_ego): - return float(np.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 +43,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 = float(np.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 +55,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 = float(np.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 = float(np.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 @@ -89,9 +89,9 @@ def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and len(model_msg.velocity.x) == ModelConstants.IDX_N and len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)) - model_error * T_IDXS_MPC - v = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)) - model_error - a = float(np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)) + x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC + v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error + a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) j = np.zeros(len(T_IDXS_MPC)) else: x = np.zeros(len(T_IDXS_MPC)) @@ -151,7 +151,7 @@ def update(self, sm): if not self.allow_throttle: clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = float(np.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: @@ -165,9 +165,9 @@ def update(self, sm): self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) - self.v_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)) - self.a_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)) - self.j_desired_trajectory = float(np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)) + self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) + self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill @@ -176,7 +176,7 @@ def update(self, sm): # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) + self.a_desired = 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): diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 906d77c2385f5d..f9142299c994af 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -49,7 +49,7 @@ def proc_name(proc): if sm.updated['deviceState']: t = sm['deviceState'] - last_temp = float(np.mean(t.cpuTempC)) + last_temp = np.mean(t.cpuTempC) last_mem = t.memoryUsagePercent if sm.updated['procLog']: @@ -72,7 +72,7 @@ def proc_name(proc): total_times = total_times_new[:] busy_times = busy_times_new[:] - print(f"CPU {100.0 * float(np.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) From 8309240af8a9defc63b4245a03737d26524d8549 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 02:35:53 +0000 Subject: [PATCH 06/13] float comversion --- selfdrive/car/tests/test_cruise_speed.py | 2 +- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 7bda3a24eb31ac..0c15c95fd2e3ab 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/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)] diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 517e3e1b04fe9f..579ee85fd44375 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -297,7 +297,7 @@ def set_cur_state(self, v, a): @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) - v_lead_traj = float(np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)) + v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8) x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj) lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv @@ -319,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 = float(np.clip(x_lead, min_x_lead, 1e8)) - v_lead = float(np.clip(v_lead, 0.0, 1e8)) - a_lead = float(np.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 @@ -358,9 +358,9 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) - v_cruise_clipped = float(np.clip(v_cruise * np.ones(N+1), + v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, - v_upper)) + v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] @@ -373,7 +373,7 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) - cruise_target = T_IDXS * float(np.clip(v_cruise, v_ego - 2.0, 1e3)) + x[0] + cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0] xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1]) x = np.cumsum(np.insert(xforward, 0, x[0])) From 351515175b7dfa952e1ef149c6bb01bb7430d4f3 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 02:51:29 +0000 Subject: [PATCH 07/13] #34331 --- selfdrive/car/cruise.py | 3 +-- selfdrive/controls/lib/drive_helpers.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 -- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index aba4cc7fcdc5a4..52a2459ad2a605 100644 --- a/selfdrive/car/cruise.py +++ b/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 -import numpy as np # WARNING: this value was determined based on the model's training distribution, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9ee2da5727b7f9..0a98e7b4cf02bc 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -1,5 +1,5 @@ -from cereal import log import numpy as np +from cereal import log from openpilot.common.realtime import DT_CTRL MIN_SPEED = 1.0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 31a467f23c693c..cd8ba39020388b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,8 +1,6 @@ #!/usr/bin/env python3 import math import numpy as np - - import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.conversions import Conversions as CV From 8669b48f61410077c8becd6d40964904b9bd89ad Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 02:59:22 +0000 Subject: [PATCH 08/13] 34331 --- selfdrive/test/longitudinal_maneuvers/plant.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index c08ac6d3692539..026c8ce22a5b32 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -121,7 +121,7 @@ def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., pitch=0.0, prob_throttle 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.] From 71a0cba98c82d8463ebc13b661e5fc0f1d861cf6 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 03:19:29 +0000 Subject: [PATCH 09/13] 34331 --- 1 | 48 ++++++++++++++++++++++ selfdrive/controls/controlsd.py | 11 ++--- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 10 ++--- 4 files changed, 60 insertions(+), 11 deletions(-) create mode 100644 1 diff --git a/1 b/1 new file mode 100644 index 00000000000000..dedc97a96471c6 --- /dev/null +++ b/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 64be4340816262..ea01cc377ab396 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -106,15 +106,16 @@ 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 +180,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) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index d3da656daa16df..4162bd62dcdd79 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index d7edd39f8a4103..dedc97a96471c6 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/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 From e44719fb2a317c6c6f972242044e505baba54b00 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 03:25:07 +0000 Subject: [PATCH 10/13] 34331 --- selfdrive/controls/controlsd.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ea01cc377ab396..6839726164a756 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -115,7 +115,8 @@ def state_control(self): 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) + actuators.steeringAngleDeg = float(steeringAngleDeg) + actuators.steer = float(steer) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: attr = getattr(actuators, p) From 05dacb4e8068e3938970c4fa276dfb4d9a354f97 Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 03:48:20 +0000 Subject: [PATCH 11/13] 34331 --- common/tests/test_numpy_fast.py | 29 ++++++++++++++++------------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/common/tests/test_numpy_fast.py b/common/tests/test_numpy_fast.py index ec2f5ea099b135..308c4df16fe919 100644 --- a/common/tests/test_numpy_fast.py +++ b/common/tests/test_numpy_fast.py @@ -1,20 +1,23 @@ import numpy as np - 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 = float(np.interp(v_ego_arr, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)) - actual = float(np.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 = 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) From d575896bdf93c4631cfab70368f67519c4ec434a Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 03:59:10 +0000 Subject: [PATCH 12/13] 34331 --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index cd8ba39020388b..51c1bacfb7f87d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -198,7 +198,7 @@ def publish(self, sm, pm): 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.allowBrake = True longitudinalPlan.allowThrottle = self.allow_throttle From 14e7cbd7c8e5f004229277b164cf8dc1a36892ac Mon Sep 17 00:00:00 2001 From: Shaikimram Date: Mon, 13 Jan 2025 06:58:23 +0000 Subject: [PATCH 13/13] 34331 --- selfdrive/controls/lib/longitudinal_planner.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 51c1bacfb7f87d..5560dfbb09ac2a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import math import numpy as np + import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.conversions import Conversions as CV @@ -137,7 +138,7 @@ def update(self, sm): 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 = float(np.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)) @@ -174,7 +175,7 @@ def update(self, sm): # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = np.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): @@ -199,8 +200,8 @@ def publish(self, sm, pm): a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) 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)