Skip to content

Commit

Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
importing numpy instead of importing specific functions
Browse files Browse the repository at this point in the history
Sammohana committed Jan 14, 2025
1 parent c094c49 commit 4624952
Showing 13 changed files with 36 additions and 37 deletions.
6 changes: 3 additions & 3 deletions selfdrive/car/cruise.py
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
import math
import numpy

from cereal import car
from openpilot.common.conversions import Conversions as CV
from numpy import clip


# 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
8 changes: 4 additions & 4 deletions selfdrive/controls/lib/drive_helpers.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
import numpy
from cereal import log
from openpilot.common.realtime import DT_CTRL
from numpy import clip

MIN_SPEED = 1.0
CONTROL_N = 17
@@ -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
4 changes: 2 additions & 2 deletions selfdrive/controls/lib/latcontrol.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
import numpy
from abc import abstractmethod, ABC

from numpy import clip
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)
6 changes: 3 additions & 3 deletions selfdrive/controls/lib/latcontrol_torque.py
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
import math
import numpy as np

from cereal import log
from opendbc.car.interfaces import LatControlInputs
from numpy 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 @@ 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
4 changes: 2 additions & 2 deletions selfdrive/controls/lib/longcontrol.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import numpy as np
from cereal import car
from numpy 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 @@ 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
7 changes: 3 additions & 4 deletions selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Original file line number Diff line number Diff line change
@@ -4,7 +4,6 @@
import numpy as np
from cereal import log
from opendbc.car.interfaces import ACCEL_MIN
from numpy 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

4 changes: 2 additions & 2 deletions selfdrive/controls/radard.py
Original file line number Diff line number Diff line change
@@ -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 numpy 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 @@ 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:
6 changes: 3 additions & 3 deletions selfdrive/debug/live_cpu_and_temp.py
Original file line number Diff line number Diff line change
@@ -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 numpy 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 @@ 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)
6 changes: 3 additions & 3 deletions selfdrive/monitoring/helpers.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
from math import atan2
import np as numpy

from cereal import car, log
import cereal.messaging as messaging
from openpilot.selfdrive.selfdrived.events import Events
from numpy 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 @@ 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

4 changes: 2 additions & 2 deletions system/hardware/fan_controller.py
Original file line number Diff line number Diff line change
@@ -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 numpy import interp
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
6 changes: 3 additions & 3 deletions tools/joystick/joystick_control.py
Original file line number Diff line number Diff line change
@@ -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 numpy 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 @@ 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:
6 changes: 3 additions & 3 deletions tools/joystick/joystickd.py
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
#!/usr/bin/env python3

import math
import numpy as np

from cereal import messaging, car
from numpy 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)
6 changes: 3 additions & 3 deletions tools/sim/bridge/common.py
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
import signal
import threading
import functools
import numpy as np

from collections import namedtuple
from enum import Enum
from multiprocessing import Process, Queue, Value
from abc import ABC, abstractmethod
from numpy import clip

from opendbc.car.honda.values import CruiseButtons
from openpilot.common.params import Params
@@ -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

0 comments on commit 4624952

Please sign in to comment.