sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,9 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.common.constants import CV
|
||||
|
||||
MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h
|
||||
@@ -0,0 +1,261 @@
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
ACTIVE_STATES = (MapState.turning, )
|
||||
ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
|
||||
|
||||
R = 6373000.0 # approximate radius of earth in meters
|
||||
TO_RADIANS = math.pi / 180
|
||||
TO_DEGREES = 180 / math.pi
|
||||
TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
|
||||
TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit
|
||||
TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps
|
||||
# reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
|
||||
# The value is multiplied against the target velocity to determine the additional distance. This is
|
||||
# done to keep the distance calculations consistent but results in the offset actually being less
|
||||
# time than specified depending on how much of a speed differential there is between v_ego and the
|
||||
# target velocity.
|
||||
|
||||
|
||||
def velocities_from_param(param: str, params: Params):
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
json_str = params.get(param)
|
||||
if json_str is None:
|
||||
return None
|
||||
|
||||
velocities = json.loads(json_str)
|
||||
|
||||
return velocities
|
||||
|
||||
|
||||
def calculate_accel(t, target_jerk, a_ego):
|
||||
return a_ego + target_jerk * t
|
||||
|
||||
|
||||
def calculate_velocity(t, target_jerk, a_ego, v_ego):
|
||||
return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
|
||||
|
||||
|
||||
def calculate_distance(t, target_jerk, a_ego, v_ego):
|
||||
return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
|
||||
|
||||
|
||||
# points should be in radians
|
||||
# output is meters
|
||||
def distance_to_point(ax, ay, bx, by):
|
||||
a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
|
||||
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
|
||||
|
||||
return R * c # in meters
|
||||
|
||||
|
||||
class SmartCruiseControlMap:
|
||||
v_target: float = 0
|
||||
a_target: float = 0.
|
||||
v_ego: float = 0.
|
||||
a_ego: float = 0.
|
||||
output_v_target: float = V_CRUISE_UNSET
|
||||
output_a_target: float = 0.
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
self.long_enabled = False
|
||||
self.long_override = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.state = MapState.disabled
|
||||
self.v_cruise = 0
|
||||
self.target_lat = 0.0
|
||||
self.target_lon = 0.0
|
||||
self.frame = -1
|
||||
|
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
|
||||
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self.is_active:
|
||||
return max(self.v_target, MIN_V)
|
||||
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def update_params(self):
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
|
||||
def update_calculations(self) -> None:
|
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
|
||||
lat = self.last_position.latitude
|
||||
lon = self.last_position.longitude
|
||||
|
||||
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
|
||||
|
||||
if self.last_position is None or self.target_velocities is None:
|
||||
return
|
||||
|
||||
min_dist = 1000
|
||||
min_idx = 0
|
||||
distances = []
|
||||
|
||||
# find our location in the path
|
||||
for i in range(len(self.target_velocities)):
|
||||
target_velocity = self.target_velocities[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
|
||||
distances.append(d)
|
||||
if d < min_dist:
|
||||
min_dist = d
|
||||
min_idx = i
|
||||
|
||||
# only look at values from our current position forward
|
||||
forward_points = self.target_velocities[min_idx:]
|
||||
forward_distances = distances[min_idx:]
|
||||
|
||||
# find velocities that we are within the distance we need to adjust for
|
||||
valid_velocities = []
|
||||
for i in range(len(forward_points)):
|
||||
target_velocity = forward_points[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
tv = target_velocity["velocity"]
|
||||
if tv > self.v_ego:
|
||||
continue
|
||||
|
||||
d = forward_distances[i]
|
||||
|
||||
a_diff = (self.a_ego - TARGET_ACCEL)
|
||||
accel_t = abs(a_diff / TARGET_JERK)
|
||||
min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
|
||||
max_d = 0
|
||||
if tv > min_accel_v:
|
||||
# calculate time needed based on target jerk
|
||||
a = 0.5 * TARGET_JERK
|
||||
b = self.a_ego
|
||||
c = self.v_ego - tv
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
|
||||
if not isinstance(t_a, complex) and t_a > 0:
|
||||
t = t_a
|
||||
else:
|
||||
t = t_b
|
||||
if isinstance(t, complex):
|
||||
continue
|
||||
|
||||
max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
else:
|
||||
t = accel_t
|
||||
max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
|
||||
# calculate additional time needed based on target accel
|
||||
t = abs((min_accel_v - tv) / TARGET_ACCEL)
|
||||
max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
|
||||
|
||||
if d < max_d + tv * TARGET_OFFSET:
|
||||
valid_velocities.append((float(tv), tlat, tlon))
|
||||
|
||||
# Find the smallest velocity we need to adjust for
|
||||
min_v = 100.0
|
||||
target_lat = 0.0
|
||||
target_lon = 0.0
|
||||
for tv, lat, lon in valid_velocities:
|
||||
if tv < min_v:
|
||||
min_v = tv
|
||||
target_lat = lat
|
||||
target_lon = lon
|
||||
|
||||
if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
|
||||
for i in range(len(forward_points)):
|
||||
target_velocity = forward_points[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
tv = target_velocity["velocity"]
|
||||
if tv > self.v_ego:
|
||||
continue
|
||||
|
||||
if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
|
||||
return
|
||||
|
||||
# not found so let's reset
|
||||
self.v_target = 0.0
|
||||
self.target_lat = 0.0
|
||||
self.target_lon = 0.0
|
||||
|
||||
self.v_target = min_v
|
||||
self.target_lat = target_lat
|
||||
self.target_lon = target_lon
|
||||
|
||||
def _update_state_machine(self) -> tuple[bool, bool]:
|
||||
# ENABLED, TURNING
|
||||
if self.state != MapState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = MapState.disabled
|
||||
elif self.long_override:
|
||||
self.state = MapState.overriding
|
||||
|
||||
else:
|
||||
# ENABLED
|
||||
if self.state == MapState.enabled:
|
||||
if self.v_cruise > self.v_target != 0:
|
||||
self.state = MapState.turning
|
||||
|
||||
# TURNING
|
||||
elif self.state == MapState.turning:
|
||||
if self.v_cruise <= self.v_target or self.v_target == 0:
|
||||
self.state = MapState.enabled
|
||||
|
||||
# OVERRIDING
|
||||
elif self.state == MapState.overriding:
|
||||
if not self.long_override:
|
||||
if self.v_cruise > self.v_target != 0:
|
||||
self.state = MapState.turning
|
||||
else:
|
||||
self.state = MapState.enabled
|
||||
|
||||
# DISABLED
|
||||
elif self.state == MapState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
if self.long_override:
|
||||
self.state = MapState.overriding
|
||||
else:
|
||||
self.state = MapState.enabled
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.long_override = long_override
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
self.v_cruise = v_cruise
|
||||
|
||||
self.update_params()
|
||||
self.update_calculations()
|
||||
|
||||
self.is_enabled, self.is_active = self._update_state_machine()
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
+19
@@ -0,0 +1,19 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
|
||||
|
||||
class SmartCruiseControl:
|
||||
def __init__(self):
|
||||
self.vision = SmartCruiseControlVision()
|
||||
self.map = SmartCruiseControlMap()
|
||||
|
||||
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
|
||||
self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
|
||||
self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
|
||||
+58
@@ -0,0 +1,58 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import platform
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
|
||||
class TestSmartCruiseControlMap:
|
||||
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.reset_params()
|
||||
self.scc_m = SmartCruiseControlMap()
|
||||
|
||||
def reset_params(self):
|
||||
self.params.put_bool("SmartCruiseControlMap", True, block=True)
|
||||
|
||||
# TODO-SP: mock data from gpsLocation
|
||||
self.params.put("LastGPSPosition", "{}", block=True)
|
||||
self.params.put("MapTargetVelocities", "{}", block=True)
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
assert not self.scc_m.is_active
|
||||
assert self.scc_m.output_v_target == V_CRUISE_UNSET
|
||||
assert self.scc_m.output_a_target == 0.
|
||||
|
||||
def test_system_disabled(self):
|
||||
self.params.put_bool("SmartCruiseControlMap", False, block=True)
|
||||
self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
assert not self.scc_m.is_active
|
||||
|
||||
def test_disabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(False, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
|
||||
def test_transition_disabled_to_enabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.enabled
|
||||
|
||||
# TODO-SP: mock data from modelV2 to test other states
|
||||
+214
@@ -0,0 +1,214 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import custom, log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
|
||||
|
||||
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
|
||||
|
||||
|
||||
def _th_above_f32(th: float) -> float:
|
||||
"""
|
||||
Return the next representable float32 *above* `th`.
|
||||
This avoids flaky comparisons around thresholds due to float32 rounding.
|
||||
"""
|
||||
th32 = np.float32(th)
|
||||
above32 = np.nextafter(th32, np.float32(np.inf), dtype=np.float32)
|
||||
return float(above32)
|
||||
|
||||
|
||||
def _build_single_spike_filtered(n: int, base: float = 1.0) -> np.ndarray:
|
||||
"""
|
||||
Create an array where max() is >= threshold but p97 is < threshold.
|
||||
This demonstrates the behavior difference vs np.amax().
|
||||
|
||||
Note: We intentionally construct using float32-representable values to match
|
||||
the data path through cereal/capnp.
|
||||
"""
|
||||
th = float(_ENTERING_PRED_LAT_ACC_TH)
|
||||
th32 = float(np.float32(th))
|
||||
|
||||
# numpy percentile default is linear interpolation: idx=(n-1)*p/100
|
||||
idx = (n - 1) * 0.97
|
||||
w = float(idx - np.floor(idx))
|
||||
|
||||
base32 = float(np.float32(base))
|
||||
|
||||
# Choose spike so that p97 = base + w*(spike-base) < th
|
||||
# -> spike < base + (th-base)/w. Use a margin (0.9) and ensure spike >= th.
|
||||
if w == 0.0:
|
||||
spike = th32 + 1.0
|
||||
else:
|
||||
spike = base32 + (th32 - base32) / w * 0.9
|
||||
spike = max(spike, th32 + 0.01)
|
||||
|
||||
arr = np.full(n, base32, dtype=np.float32)
|
||||
arr[-1] = np.float32(spike)
|
||||
return arr
|
||||
|
||||
|
||||
def generate_modelV2():
|
||||
model = messaging.new_message('modelV2')
|
||||
position = log.XYZTData.new_message()
|
||||
speed = 30
|
||||
position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
model.modelV2.position = position
|
||||
orientation = log.XYZTData.new_message()
|
||||
curvature = 0.05
|
||||
orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
|
||||
orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
|
||||
model.modelV2.orientation = orientation
|
||||
orientationRate = log.XYZTData.new_message()
|
||||
orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
|
||||
model.modelV2.orientationRate = orientationRate
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity.x[0] = float(speed) # always start at current speed
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
|
||||
return model
|
||||
|
||||
|
||||
def generate_carState():
|
||||
car_state = messaging.new_message('carState')
|
||||
speed = 30
|
||||
v_cruise = 50
|
||||
car_state.carState.vEgo = float(speed)
|
||||
car_state.carState.standstill = False
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
|
||||
return car_state
|
||||
|
||||
|
||||
def generate_controlsState():
|
||||
controls_state = messaging.new_message('controlsState')
|
||||
controls_state.controlsState.curvature = 0.05
|
||||
|
||||
return controls_state
|
||||
|
||||
|
||||
class TestSmartCruiseControlVision:
|
||||
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.reset_params()
|
||||
self.scc_v = SmartCruiseControlVision()
|
||||
|
||||
mdl = generate_modelV2()
|
||||
cs = generate_carState()
|
||||
controls_state = generate_controlsState()
|
||||
self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
|
||||
|
||||
def reset_params(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", True, block=True)
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
assert self.scc_v.output_v_target == V_CRUISE_UNSET
|
||||
assert self.scc_v.output_a_target == 0.
|
||||
|
||||
def test_system_disabled(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", False, block=True)
|
||||
self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
|
||||
def test_disabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, False, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
|
||||
def test_transition_disabled_to_enabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"case, should_enter",
|
||||
[
|
||||
("p97_just_above_threshold", True),
|
||||
("single_spike_filtered", False),
|
||||
("persistent_high_values", True),
|
||||
],
|
||||
ids=[
|
||||
"p97>threshold_enters",
|
||||
"single_spike_max_large_but_p97_below_threshold",
|
||||
"high_values_persist_trigger_entering",
|
||||
],
|
||||
)
|
||||
def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
|
||||
n = len(ModelConstants.T_IDXS)
|
||||
th = float(_ENTERING_PRED_LAT_ACC_TH)
|
||||
|
||||
if case == "p97_just_above_threshold":
|
||||
# Use the next representable float32 above threshold to avoid float32 rounding flakiness.
|
||||
val = _th_above_f32(th)
|
||||
pred_lat_accels = np.full(n, np.float32(val), dtype=np.float32)
|
||||
|
||||
elif case == "single_spike_filtered":
|
||||
pred_lat_accels = _build_single_spike_filtered(n, base=1.0)
|
||||
|
||||
elif case == "persistent_high_values":
|
||||
# Make enough "high" samples so p97 is driven by the persistent trend, not a single outlier.
|
||||
high_count = max(2, int(np.ceil(n * 0.03)) + 1)
|
||||
pred_lat_accels = np.full(n, np.float32(1.0), dtype=np.float32)
|
||||
pred_lat_accels[-high_count:] = np.float32(2.0)
|
||||
pred_lat_accels[-1] = np.float32(8.0) # keep one big outlier too
|
||||
|
||||
else:
|
||||
raise AssertionError(f"Unknown case: {case}")
|
||||
|
||||
# Override model predictions so:
|
||||
# predicted_lat_accels = abs(orientationRate.z) * velocity.x == pred_lat_accels
|
||||
mdl = generate_modelV2()
|
||||
mdl.modelV2.velocity.x = [1.0 for _ in range(n)]
|
||||
mdl.modelV2.orientationRate.z = [float(x) for x in pred_lat_accels]
|
||||
self.sm["modelV2"] = mdl.modelV2
|
||||
|
||||
v_ego = float(MIN_V + 5.0)
|
||||
|
||||
# 1st update: disabled -> enabled
|
||||
self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
|
||||
# 2nd update: evaluate entering condition from enabled state
|
||||
self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
|
||||
|
||||
# Controller does percentile on numpy float64 arrays (values already quantized by capnp),
|
||||
# so compute expected in float64 to match behavior and avoid interpolation/rounding deltas.
|
||||
expected_p97 = float(np.percentile(pred_lat_accels.astype(np.float64), 97))
|
||||
|
||||
# allow tiny numeric differences due to float conversions/interpolation
|
||||
assert np.isclose(self.scc_v.max_pred_lat_acc, expected_p97, rtol=1e-6, atol=1e-5)
|
||||
|
||||
if should_enter:
|
||||
# We assert entering primarily by state (this is the actual intended behavior).
|
||||
assert self.scc_v.state == VisionState.entering
|
||||
# Optional sanity: should be >= threshold with some margin (since we used nextafter above threshold).
|
||||
assert self.scc_v.max_pred_lat_acc > th
|
||||
|
||||
else:
|
||||
# Difference vs np.amax(): max can be above threshold, but p97 stays below it.
|
||||
assert float(np.max(pred_lat_accels)) >= th
|
||||
assert self.scc_v.max_pred_lat_acc < th
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
|
||||
# TODO-SP: mock modelV2 data to test other states
|
||||
+203
@@ -0,0 +1,203 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
|
||||
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
|
||||
|
||||
ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
|
||||
ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
|
||||
|
||||
_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state.
|
||||
_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops.
|
||||
|
||||
_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state.
|
||||
|
||||
_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state.
|
||||
_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle.
|
||||
|
||||
_A_LAT_REG_MAX = 2. # Maximum lateral acceleration
|
||||
|
||||
_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting.
|
||||
|
||||
# Lookup table for the minimum smooth deceleration during the ENTERING state
|
||||
# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
|
||||
_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state
|
||||
_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead
|
||||
|
||||
# Lookup table for the acceleration for the TURNING state
|
||||
# depending on the current lateral acceleration of the vehicle.
|
||||
_TURNING_ACC_V = [0.5, 0., -0.4] # acc value
|
||||
_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc
|
||||
|
||||
_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn.
|
||||
|
||||
|
||||
class SmartCruiseControlVision:
|
||||
v_target: float = 0
|
||||
a_target: float = 0.
|
||||
v_ego: float = 0.
|
||||
a_ego: float = 0.
|
||||
output_v_target: float = V_CRUISE_UNSET
|
||||
output_a_target: float = 0.
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
self.long_enabled = False
|
||||
self.long_override = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
self.v_cruise_setpoint = 0.
|
||||
|
||||
self.state = VisionState.disabled
|
||||
self.current_lat_acc = 0.
|
||||
self.max_pred_lat_acc = 0.
|
||||
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_target
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self.is_active:
|
||||
return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
|
||||
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
def _update_params(self) -> None:
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
|
||||
def _update_calculations(self, sm: messaging.SubMaster) -> None:
|
||||
if not self.long_enabled:
|
||||
return
|
||||
else:
|
||||
rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
|
||||
vel_plan = np.array(sm['modelV2'].velocity.x)
|
||||
|
||||
self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
|
||||
|
||||
# get the maximum lat accel from the model
|
||||
predicted_lat_accels = rate_plan * vel_plan
|
||||
self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
|
||||
|
||||
# get the maximum curve based on the current velocity
|
||||
v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations
|
||||
max_curve = self.max_pred_lat_acc / (v_ego**2)
|
||||
|
||||
# Get the target velocity for the maximum curve
|
||||
self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
|
||||
|
||||
def _update_state_machine(self) -> tuple[bool, bool]:
|
||||
# ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
|
||||
if self.state != VisionState.disabled:
|
||||
# longitudinal and feature disable always have priority in a non-disabled state
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = VisionState.disabled
|
||||
elif self.long_override:
|
||||
self.state = VisionState.overriding
|
||||
|
||||
else:
|
||||
# ENABLED
|
||||
if self.state == VisionState.enabled:
|
||||
# Do not enter a turn control cycle if the speed is low.
|
||||
if self.v_ego <= MIN_V:
|
||||
pass
|
||||
# If significant lateral acceleration is predicted ahead, then move to Entering turn state.
|
||||
elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.entering
|
||||
|
||||
# OVERRIDING
|
||||
elif self.state == VisionState.overriding:
|
||||
if not self.long_override:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# ENTERING
|
||||
elif self.state == VisionState.entering:
|
||||
# Transition to Turning if current lateral acceleration is over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Abort if the predicted lateral acceleration drops
|
||||
elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# Transition to Leaving if current lateral acceleration drops below a threshold.
|
||||
if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
|
||||
self.state = VisionState.leaving
|
||||
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# Transition back to Turning if current lateral acceleration goes back over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Finish if current lateral acceleration goes below a threshold.
|
||||
elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# DISABLED
|
||||
elif self.state == VisionState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
if self.long_override:
|
||||
self.state = VisionState.overriding
|
||||
else:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def _update_solution(self) -> float:
|
||||
# DISABLED, ENABLED, OVERRIDING
|
||||
if self.state not in ACTIVE_STATES:
|
||||
# when not overshooting, calculate v_turn as the speed at the prediction horizon when following
|
||||
# the smooth deceleration.
|
||||
a_target = self.a_ego
|
||||
# ENTERING
|
||||
elif self.state == VisionState.entering:
|
||||
# when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
|
||||
a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
|
||||
a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# When leaving, we provide a comfortable acceleration to regain speed.
|
||||
a_target = _LEAVING_ACC
|
||||
else:
|
||||
raise NotImplementedError(f"SCC-V state not supported: {self.state}")
|
||||
|
||||
return a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
|
||||
v_cruise_setpoint: float) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.long_override = long_override
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
self.v_cruise_setpoint = v_cruise_setpoint
|
||||
|
||||
self._update_params()
|
||||
self._update_calculations(sm)
|
||||
|
||||
self.is_enabled, self.is_active = self._update_state_machine()
|
||||
self.a_target = self._update_solution()
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
Reference in new issue
Block a user