mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-03 03:53:42 +08:00
feat(long): acceleration profiles
This commit is contained in:
rav4kumar
@@ -204,11 +204,16 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelController @8 :AccelController;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
decelIntent @3 :Float32;
|
||||
curveDetected @4 :Bool;
|
||||
wantBlended @5 :Bool;
|
||||
leadVeto @6 :Bool;
|
||||
|
||||
enum DynamicExperimentalControlState {
|
||||
acc @0;
|
||||
@@ -306,6 +311,17 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
greenLightAlert @0 :Bool;
|
||||
leadDepartAlert @1 :Bool;
|
||||
}
|
||||
|
||||
struct AccelController {
|
||||
enabled @0 :Bool;
|
||||
active @1 :Bool;
|
||||
profile @2 :Profile;
|
||||
enum Profile {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -249,6 +249,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// Accel Controller profiles (Eco / Normal / Sport)
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
|
||||
|
||||
// sunnypilot model params
|
||||
{"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
@@ -117,12 +117,16 @@ class TestParams(OpenpilotTestCase):
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("AccelPersonalityEnabled")
|
||||
self.params.remove("AccelPersonality")
|
||||
self.params.remove("LiveParametersV2")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
|
||||
assert self.params.get("AccelPersonality", return_default=True) == 1
|
||||
assert self.params.get("LiveParametersV2") is None
|
||||
assert self.params.get("LiveParametersV2", return_default=True) is None
|
||||
|
||||
|
||||
@@ -35,9 +35,12 @@ def get_max_accel(v_ego):
|
||||
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
|
||||
|
||||
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
|
||||
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
|
||||
|
||||
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
|
||||
max_accel_override=None):
|
||||
if max_accel_override is not None:
|
||||
max_accel = max_accel_override
|
||||
else:
|
||||
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
|
||||
if not e2e:
|
||||
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)
|
||||
@@ -84,7 +87,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
v_ego = sm['carState'].vEgo
|
||||
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
|
||||
v_cruise = v_cruise_kph * CV.KPH_TO_MS
|
||||
if sm['controlsState'].forceDecel:
|
||||
force_decel = sm['controlsState'].forceDecel
|
||||
if force_decel:
|
||||
v_cruise = 0.0
|
||||
|
||||
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
|
||||
@@ -118,6 +122,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
|
||||
self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
|
||||
self.update_dec(sm)
|
||||
|
||||
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)
|
||||
@@ -140,9 +145,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
|
||||
is_e2e = self.is_e2e(sm)
|
||||
|
||||
self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
|
||||
self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
|
||||
accel_coast, self.allow_throttle)
|
||||
max_accel_override = self.get_max_accel_override(v_ego, v_cruise, is_e2e)
|
||||
a_cruise_prev = self.a_cruise
|
||||
gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
|
||||
self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
|
||||
ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
|
||||
self.CP, self.dt, accel_coast, True, max_accel_override)
|
||||
self.a_cruise = self.arbitrate_cruise_candidate(
|
||||
sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
|
||||
allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel,
|
||||
)
|
||||
cruise_should_stop = should_stop(v_ego, self.a_cruise)
|
||||
|
||||
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
|
||||
@@ -177,6 +189,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
longitudinalPlan.aTarget = float(self.output_a_target)
|
||||
longitudinalPlan.shouldStop = bool(self.output_should_stop)
|
||||
longitudinalPlan.allowBrake = True
|
||||
# Raw model throttle intent used for path visualization; lead MPC can still request positive acceleration.
|
||||
longitudinalPlan.allowThrottle = bool(self.allow_throttle)
|
||||
|
||||
pm.send('longitudinalPlan', plan_send)
|
||||
|
||||
@@ -11,6 +11,15 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, radar_frame: int, services: dict):
|
||||
super().__init__(services)
|
||||
self.frame = radar_frame
|
||||
self.logMonoTime = {"radarState": radar_frame}
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
|
||||
|
||||
class Plant:
|
||||
messaging_initialized = False
|
||||
|
||||
@@ -132,7 +141,7 @@ class Plant:
|
||||
car_control.carControl.orientationNED = [0., float(pitch), 0.]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = {'radarState': radar.radarState,
|
||||
sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
@@ -141,7 +150,7 @@ class Plant:
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation}
|
||||
'gpsLocation': gps_data.gpsLocation})
|
||||
self.planner.update(sm)
|
||||
self.acceleration = self.planner.output_a_target
|
||||
if self.planner.output_should_stop:
|
||||
|
||||
@@ -27,6 +27,12 @@ DESCRIPTIONS = {
|
||||
"In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
|
||||
"your steering wheel distance button."
|
||||
),
|
||||
"AccelPersonalityEnabled": tr_noop(
|
||||
"Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged."
|
||||
),
|
||||
"AccelPersonality": tr_noop(
|
||||
"Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive."
|
||||
),
|
||||
"IsLdwEnabled": tr_noop(
|
||||
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
|
||||
"without a turn signal activated while driving over 31 mph (50 km/h)."
|
||||
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._accel_controller_enabled = toggle_item(
|
||||
lambda: tr("Enable Accel Controller"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
|
||||
self._params.get_bool("AccelPersonalityEnabled"),
|
||||
callback=self._set_accel_controller_enabled,
|
||||
icon="speed_limit.png",
|
||||
)
|
||||
|
||||
self._accel_personality_setting = multiple_button_item(
|
||||
lambda: tr("Acceleration Profile"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonality"]),
|
||||
buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
|
||||
button_width=300,
|
||||
callback=self._set_accel_personality,
|
||||
selected_index=self._params.get("AccelPersonality", return_default=True),
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._toggles = {}
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):
|
||||
|
||||
self._toggles[param] = toggle
|
||||
|
||||
# insert longitudinal personality after NDOG toggle
|
||||
# insert longitudinal personality and Accel Controller settings after NDOG toggle
|
||||
if param == "DisengageOnAccelerator":
|
||||
self._toggles["LongitudinalPersonality"] = self._long_personality_setting
|
||||
self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
|
||||
self._toggles["AccelPersonality"] = self._accel_personality_setting
|
||||
|
||||
self._update_experimental_mode_icon()
|
||||
self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
|
||||
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
e2e_description = tr(
|
||||
"sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
|
||||
@@ -176,11 +203,15 @@ class TogglesLayout(Widget):
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(True)
|
||||
self._toggles["ExperimentalMode"].set_description(e2e_description)
|
||||
self._long_personality_setting.action_item.set_enabled(True)
|
||||
self._accel_controller_enabled.action_item.set_enabled(True)
|
||||
self._accel_personality_setting.action_item.set_enabled(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(False)
|
||||
self._toggles["ExperimentalMode"].action_item.set_state(False)
|
||||
self._long_personality_setting.action_item.set_enabled(False)
|
||||
self._accel_controller_enabled.action_item.set_enabled(False)
|
||||
self._accel_personality_setting.action_item.set_enabled(False)
|
||||
self._params.remove("ExperimentalMode")
|
||||
|
||||
unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
|
||||
@@ -203,6 +234,8 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
|
||||
self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -247,3 +280,9 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality(self, button_index: int):
|
||||
self._params.put("AccelPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_controller_enabled(self, state: bool):
|
||||
self._params.put_bool("AccelPersonalityEnabled", state, block=True)
|
||||
|
||||
@@ -42,6 +42,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
super().__init__()
|
||||
|
||||
self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
|
||||
self._accel_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
|
||||
self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
|
||||
self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
|
||||
toggle_callback=self._on_experimental_mode)
|
||||
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
|
||||
@@ -53,6 +55,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
self._scroller.add_widgets([
|
||||
self._personality_toggle,
|
||||
self._accel_controller_enabled,
|
||||
self._accel_personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
ldw_toggle,
|
||||
@@ -65,6 +69,7 @@ class TogglesLayoutMici(NavScroller):
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("AccelPersonalityEnabled", self._accel_controller_enabled),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
("AlwaysOnDM", always_on_dm_toggle),
|
||||
@@ -104,17 +109,23 @@ class TogglesLayoutMici(NavScroller):
|
||||
if ui_state.has_longitudinal_control:
|
||||
self._experimental_btn.set_visible(True)
|
||||
self._personality_toggle.set_visible(True)
|
||||
self._accel_controller_enabled.set_visible(True)
|
||||
self._accel_personality_toggle.set_visible(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._experimental_btn.set_visible(False)
|
||||
self._experimental_btn.set_checked(False)
|
||||
self._personality_toggle.set_visible(False)
|
||||
self._accel_controller_enabled.set_visible(False)
|
||||
self._accel_personality_toggle.set_visible(False)
|
||||
ui_state.params.remove("ExperimentalMode")
|
||||
|
||||
# Refresh toggles from params to mirror external changes
|
||||
for key, item in self._refresh_toggles:
|
||||
item.set_checked(ui_state.params.get_bool(key))
|
||||
|
||||
self._accel_personality_toggle.refresh()
|
||||
|
||||
def _on_experimental_mode(self, state: bool):
|
||||
if state and not ui_state.params.get_bool("ExperimentalModeConfirmed"):
|
||||
# Don't show enabled state until confirm
|
||||
|
||||
@@ -383,13 +383,18 @@ class BigMultiParamToggle(BigMultiToggle):
|
||||
self._load_value()
|
||||
|
||||
def _load_value(self):
|
||||
self.set_value(self._options[self._params.get(self._param) or 0])
|
||||
value = self._params.get(self._param, return_default=True)
|
||||
index = value if isinstance(value, int) else 0
|
||||
self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
new_idx = self._options.index(self.value)
|
||||
self._params.put(self._param, new_idx)
|
||||
|
||||
def refresh(self):
|
||||
self._load_value()
|
||||
|
||||
|
||||
class BigParamControl(BigToggle):
|
||||
def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
"""
|
||||
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
|
||||
|
||||
from openpilot.cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import get_sanitize_int_param
|
||||
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 10., 20., 25., 40.] # m/s
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.60, 1.48, 1.22, 0.86, 0.66, 0.52, 0.40],
|
||||
AccelProfile.normal: [1.90, 1.70, 1.42, 0.99, 0.80, 0.66, 0.52],
|
||||
AccelProfile.sport: [2.00, 2.00, 1.86, 1.30, 1.02, 0.86, 0.72],
|
||||
}
|
||||
|
||||
|
||||
class AccelController:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = 0
|
||||
self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
def update(self) -> None:
|
||||
self.frame += 1
|
||||
if self.frame % int(1.0 / DT_MDL) == 0:
|
||||
self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
@property
|
||||
def profile(self) -> int:
|
||||
return self._profile
|
||||
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
+320
@@ -0,0 +1,320 @@
|
||||
"""
|
||||
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
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MAX
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import (
|
||||
A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
)
|
||||
|
||||
|
||||
class TestAccelController(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
|
||||
def set_profile(self, profile: int) -> AccelController:
|
||||
self.params.put("AccelPersonality", profile, block=True)
|
||||
return AccelController()
|
||||
|
||||
def test_table_breakpoints(self):
|
||||
for profile, values in MAX_ACCEL_PROFILES.items():
|
||||
controller = self.set_profile(profile)
|
||||
for speed, expected in zip(MAX_ACCEL_BREAKPOINTS, values, strict=True):
|
||||
assert controller.get_max_accel(speed) == expected
|
||||
|
||||
def test_profile_ordering_and_bounds(self):
|
||||
controllers = {
|
||||
AccelProfile.eco: self.set_profile(AccelProfile.eco),
|
||||
AccelProfile.normal: self.set_profile(AccelProfile.normal),
|
||||
AccelProfile.sport: self.set_profile(AccelProfile.sport),
|
||||
}
|
||||
previous = {profile: float("inf") for profile in controllers}
|
||||
|
||||
for speed in np.linspace(0.0, 55.0, 551):
|
||||
values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()}
|
||||
assert 0.0 <= values[AccelProfile.eco] <= values[AccelProfile.normal] <= values[AccelProfile.sport] <= 2.0
|
||||
for profile, value in values.items():
|
||||
assert value <= previous[profile]
|
||||
previous[profile] = value
|
||||
|
||||
def test_profiles_stay_within_openpilot_accel_max(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
controller = self.set_profile(profile)
|
||||
for speed in np.linspace(0.0, 55.0, 551):
|
||||
assert controller.get_max_accel(speed) <= ACCEL_MAX
|
||||
|
||||
def test_profiles_have_material_separation(self):
|
||||
controllers = [self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)]
|
||||
for speed in MAX_ACCEL_BREAKPOINTS:
|
||||
eco, normal, sport = (controller.get_max_accel(speed) for controller in controllers)
|
||||
assert normal - eco >= 0.1 - 1e-12
|
||||
assert sport - normal >= 0.1 - 1e-12
|
||||
for speed in MAX_ACCEL_BREAKPOINTS[1:-1]:
|
||||
assert controllers[2].get_max_accel(speed) - controllers[0].get_max_accel(speed) >= 0.3 - 1e-12
|
||||
|
||||
def test_profiles_keep_usable_road_speed_acceleration(self):
|
||||
# A previous revision had eco at 0.20 m/s^2 at 40 m/s. 1% of road grade costs 0.098 m/s^2 of gravity, so
|
||||
# that profile cannot hold speed on anything steeper than ~2% and can never recover once it bleeds off.
|
||||
# This is a LOWER bound on purpose: the tapered upper bounds it replaces let highway accel go to zero.
|
||||
controllers = {profile: self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
|
||||
for speed in np.linspace(8.0, 40.0, 321):
|
||||
stock = float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS))
|
||||
values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()}
|
||||
# 0.35 m/s^2 holds a 3% grade; the fractions keep merges and passes usable.
|
||||
assert values[AccelProfile.eco] >= max(0.35, 0.60 * stock), speed
|
||||
assert values[AccelProfile.normal] >= 0.80 * stock, speed
|
||||
assert values[AccelProfile.sport] >= stock, speed
|
||||
|
||||
def test_eco_never_exceeds_stock(self):
|
||||
controller = self.set_profile(AccelProfile.eco)
|
||||
for speed in np.linspace(0.0, 55.0, 551):
|
||||
assert controller.get_max_accel(speed) <= float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) + 1e-12, speed
|
||||
|
||||
def test_comfort_profile_caps_taper_after_launch(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal):
|
||||
values = MAX_ACCEL_PROFILES[profile]
|
||||
assert values[3] <= 0.55 * values[0]
|
||||
|
||||
def test_sport_uses_openpilot_accel_max_at_launch(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
assert controller.get_max_accel(0.0) == ACCEL_MAX
|
||||
assert all(controller.get_max_accel(speed) <= ACCEL_MAX for speed in np.linspace(0.0, 55.0, 551))
|
||||
|
||||
def test_ceiling_is_continuous_in_speed(self):
|
||||
# The ceiling is the only thing the controller sets, so a step in it is a step in the commanded
|
||||
# acceleration. dt=10 makes the stock jerk limiter a no-op so nothing can hide a discontinuity.
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
controller = self.set_profile(profile)
|
||||
speeds = np.linspace(0.0, 45.0, 451)
|
||||
spacing = float(speeds[1] - speeds[0])
|
||||
commands = np.asarray([
|
||||
get_cruise_accel(False, 60.0, speed, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, controller.get_max_accel(speed))
|
||||
for speed in speeds
|
||||
])
|
||||
assert np.all(np.isfinite(commands)), profile
|
||||
assert np.all(np.abs(np.diff(commands)) <= spacing * 1.05 + 1e-9), profile
|
||||
|
||||
def test_negative_speed_uses_standstill_value(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
assert controller.get_max_accel(-1.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][0]
|
||||
|
||||
def test_profile_change_has_no_controller_filter(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
controller.frame = int(1.0 / DT_MDL) - 1
|
||||
controller.update()
|
||||
index = MAX_ACCEL_BREAKPOINTS.index(10.0)
|
||||
assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][index]
|
||||
|
||||
def test_params_refresh_once_per_second(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
controller.update()
|
||||
assert controller.profile == AccelProfile.normal
|
||||
controller.frame = int(1.0 / DT_MDL) - 1
|
||||
controller.update()
|
||||
assert controller.profile == AccelProfile.sport
|
||||
|
||||
def test_enabled_param_refresh(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
controller.frame = int(1.0 / DT_MDL) - 1
|
||||
controller.update()
|
||||
assert not controller.is_enabled()
|
||||
|
||||
|
||||
class TestPlannerIntegration(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
|
||||
def test_none_override_matches_stock(self):
|
||||
for e2e in (False, True):
|
||||
for allow_throttle in (False, True):
|
||||
args = (e2e, 30.0, 12.0, 0.2, 4.0, _fake_cp(), DT_MDL, -0.3, allow_throttle)
|
||||
assert get_cruise_accel(*args) == get_cruise_accel(*args, max_accel_override=None)
|
||||
|
||||
def test_profiles_do_not_change_far_braking(self):
|
||||
# The ceiling is an upper bound only, so it can never participate in a deceleration. Braking authority
|
||||
# stays with stock's clip to A_CRUISE_MIN for every profile.
|
||||
args = (False, 0.0, 20.0, 0.0, 0.0, _fake_cp(), 10.0, -0.3, True)
|
||||
stock = get_cruise_accel(*args)
|
||||
assert stock == A_CRUISE_MIN
|
||||
for profile_values in MAX_ACCEL_PROFILES.values():
|
||||
assert get_cruise_accel(*args, max_accel_override=profile_values[0]) == stock
|
||||
|
||||
def test_stock_jerk_limit_still_owns_smoothing(self):
|
||||
speed = 8.0
|
||||
sport_limit = np.interp(speed, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport])
|
||||
target = get_cruise_accel(False, 30.0, speed, 0.0, 0.0, _fake_cp(), DT_MDL, 0.0, True, sport_limit)
|
||||
jerk_limit = np.interp(speed, A_CRUISE_MAX_BP, J_CRUISE_VALS) * DT_MDL
|
||||
assert np.isclose(target, jerk_limit)
|
||||
|
||||
def test_disabled_leaves_stock_limit_active(self):
|
||||
planner = _bare_planner()
|
||||
for e2e in (False, True):
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=e2e) is None
|
||||
assert planner.accel_controller_active is False
|
||||
|
||||
def test_e2e_uses_enabled_profile(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=True) == expected
|
||||
assert planner.accel_controller_active is True
|
||||
|
||||
def test_enabled_acc_uses_python_native_telemetry_types(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == expected
|
||||
assert type(planner.accel_controller_active) is bool
|
||||
assert type(planner.accel_controller.is_enabled()) is bool
|
||||
assert type(planner.accel_controller.profile) is int
|
||||
|
||||
def test_normal_profile_uses_tuned_limit(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == expected
|
||||
assert planner.accel_controller_active is True
|
||||
|
||||
def test_ceiling_applies_to_every_target_source(self):
|
||||
# The ceiling is speed-scheduled only, so it is deliberately source-independent. This is what the old
|
||||
# COMFORT_SOURCES allow-list existed to qualify; with target shaping gone there is nothing to gate,
|
||||
# because an upper bound on acceleration cannot soften an SCC or speed-limit deceleration.
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
speed = 29.0
|
||||
expected = planner.accel_controller.get_max_accel(speed)
|
||||
|
||||
for source in (LongitudinalPlanSource.cruise, LongitudinalPlanSource.sccVision,
|
||||
LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist):
|
||||
planner.source = source
|
||||
assert np.isclose(planner.get_max_accel_override(speed, 33.0, e2e=False), expected), source
|
||||
|
||||
def test_carried_accel_state_cannot_ratchet_above_the_ceiling(self):
|
||||
# get_cruise_accel clips to max_accel FIRST and applies its jerk limit SECOND, so when
|
||||
# a_cruise_prev - j*dt is above the ceiling, that second clip's lower bound pulls the command back over
|
||||
# it and can only walk down at j_cruise. a_cruise is force-set to the measured aEgo on reset_state, so
|
||||
# after the driver accelerates hard and lifts off, openpilot re-engages pinned above the profile.
|
||||
# Measured on route 000005dd: 87 frames commanding up to 1.70 m/s^2 where eco allows 0.87.
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
planner = _bare_planner()
|
||||
v_ego = 9.84
|
||||
ceiling = planner.accel_controller.get_max_accel(v_ego)
|
||||
|
||||
planner.a_cruise = 1.90 # what a hard driver launch leaves behind
|
||||
override = planner.get_max_accel_override(v_ego, 30.0, e2e=False)
|
||||
|
||||
assert np.isclose(override, ceiling)
|
||||
assert planner.a_cruise <= ceiling + 1e-12
|
||||
accel = get_cruise_accel(False, 30.0, v_ego, planner.a_cruise, 0.0, _fake_cp(), DT_MDL, 0.0, True, override)
|
||||
assert accel <= ceiling + 1e-12
|
||||
|
||||
# Braking must be untouched: the clamp is upper-side only.
|
||||
for carried in (-3.5, -1.2, -0.4, 0.0):
|
||||
planner.a_cruise = carried
|
||||
planner.get_max_accel_override(v_ego, 30.0, e2e=False)
|
||||
assert planner.a_cruise == carried, carried
|
||||
|
||||
# Disabled must not touch the carried state at all.
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
off = _bare_planner()
|
||||
off.a_cruise = 1.90
|
||||
assert off.get_max_accel_override(v_ego, 30.0, e2e=False) is None
|
||||
assert off.a_cruise == 1.90
|
||||
|
||||
def test_e2e_candidate_is_held_through_a_brake_but_not_otherwise(self):
|
||||
# Route 000005dd: e2e -> lead1 stepped +2.25 m/s^2 in one frame (45 m/s^3) and back the next, while the
|
||||
# model held desiredAcceleration at -1.63 and never moved more than 0.024. Dropping a candidate the model
|
||||
# still owns is what produced the brake/gas/brake flip.
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import E2E_BRAKE_HOLD_ACCEL, MpcPlanSource
|
||||
|
||||
planner = _bare_planner()
|
||||
|
||||
class _Mpc:
|
||||
source = MpcPlanSource.cruise
|
||||
|
||||
class _Dec:
|
||||
def __init__(self):
|
||||
self._active = True
|
||||
self._mode = "acc"
|
||||
|
||||
def active(self):
|
||||
return self._active
|
||||
|
||||
def mode(self):
|
||||
return self._mode
|
||||
|
||||
planner.mpc = _Mpc()
|
||||
planner.dec = _Dec()
|
||||
|
||||
def sm(experimental: bool, model_accel: float):
|
||||
return {
|
||||
'selfdriveState': type("S", (), {"experimentalMode": experimental})(),
|
||||
'modelV2': type("M", (), {"action": type("A", (), {"desiredAcceleration": model_accel})()})(),
|
||||
}
|
||||
|
||||
braking = E2E_BRAKE_HOLD_ACCEL - 1.0
|
||||
|
||||
# Not experimental: never e2e, whatever the model wants.
|
||||
assert planner.is_e2e(sm(False, braking)) is False
|
||||
|
||||
# DEC in acc, and the model was NOT the selected source: acc stands. This is the case that must stay
|
||||
# untouched, or a phantom model brake could be pulled into the arbitration that never won it.
|
||||
planner.mpc.source = MpcPlanSource.lead0
|
||||
assert planner.is_e2e(sm(True, braking)) is False
|
||||
|
||||
# DEC in acc, model WAS selected and is still braking: hold it rather than release the brake.
|
||||
planner.mpc.source = MpcPlanSource.e2e
|
||||
assert planner.is_e2e(sm(True, braking)) is True
|
||||
|
||||
# Still selected but no longer braking: release, DEC's decision stands.
|
||||
assert planner.is_e2e(sm(True, 0.0)) is False
|
||||
assert planner.is_e2e(sm(True, E2E_BRAKE_HOLD_ACCEL + 0.01)) is False
|
||||
|
||||
# DEC blended, or DEC inactive, is unconditionally e2e as before.
|
||||
planner.dec._mode = "blended"
|
||||
assert planner.is_e2e(sm(True, 1.0)) is True
|
||||
planner.dec._mode = "acc"
|
||||
planner.dec._active = False
|
||||
assert planner.is_e2e(sm(True, 1.0)) is True
|
||||
|
||||
|
||||
def _fake_cp():
|
||||
class CP:
|
||||
steerRatio = 15.0
|
||||
wheelbase = 2.7
|
||||
|
||||
return CP()
|
||||
|
||||
|
||||
def _bare_planner():
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
|
||||
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_controller = AccelController()
|
||||
planner.accel_controller_active = False
|
||||
planner.allow_throttle = True
|
||||
planner.a_cruise = 0.0
|
||||
planner.source = LongitudinalPlanSource.cruise
|
||||
return planner
|
||||
+279
@@ -0,0 +1,279 @@
|
||||
"""
|
||||
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 collections.abc import Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, J_CRUISE_VALS, get_cruise_accel
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, T_IDXS as T_IDXS_MPC
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
|
||||
|
||||
|
||||
class CarParams:
|
||||
steerRatio = 15.0
|
||||
wheelbase = 2.7
|
||||
|
||||
|
||||
def _set_mpc_acceleration(plant: PlantSP, acceleration: float = 2.0) -> None:
|
||||
def update(_radar_state, **_kwargs):
|
||||
mpc = plant.planner.mpc
|
||||
mpc.source = LongitudinalPlanSource.lead0
|
||||
mpc.v_solution[:] = mpc.x0[1] + acceleration * T_IDXS_MPC
|
||||
mpc.a_solution.fill(acceleration)
|
||||
mpc.j_solution.fill(0.0)
|
||||
|
||||
plant.planner.mpc.update = update
|
||||
|
||||
|
||||
def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cruise: float = 30.0,
|
||||
v_cruise_fn: Callable[[int], float] | None = None, e2e: bool = False, steps: int = 120,
|
||||
speed_noise: float = 0.0, seed: int = 0):
|
||||
params = Params()
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
params.put("AccelPersonality", profile, block=True)
|
||||
controller = AccelController()
|
||||
rng = np.random.default_rng(seed)
|
||||
|
||||
accel = 0.0
|
||||
rows = []
|
||||
for frame in range(steps):
|
||||
target_speed = v_cruise if v_cruise_fn is None else v_cruise_fn(frame)
|
||||
measured = speed + (float(rng.normal(0.0, speed_noise)) if speed_noise else 0.0)
|
||||
max_accel_override = controller.get_max_accel(measured) if controller.is_enabled() else None
|
||||
accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True, max_accel_override)
|
||||
speed = max(0.0, speed + accel * DT_MDL)
|
||||
rows.append((speed, accel, should_stop(speed, accel)))
|
||||
return rows
|
||||
|
||||
|
||||
def run_vehicle_profile(profile: int, duration: float = 80.0, enabled: bool = True):
|
||||
params = Params()
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
params.put("AccelPersonality", profile, block=True)
|
||||
|
||||
plant = PlantSP(speed=0.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
|
||||
_set_mpc_acceleration(plant)
|
||||
rows = []
|
||||
while plant.current_time < duration:
|
||||
result = plant.step(v_cruise=25.0)
|
||||
rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"]))
|
||||
return np.asarray(rows)
|
||||
|
||||
|
||||
class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
def test_profiles_are_immediate_smooth_and_clearly_distinct(self):
|
||||
traces = {profile: run_vehicle_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
|
||||
stock = run_vehicle_profile(AccelProfile.normal, enabled=False)
|
||||
|
||||
def crossing(trace, speed):
|
||||
return float(trace[np.flatnonzero(trace[:, 1] >= speed)[0], 0])
|
||||
|
||||
time_to_20 = {profile: crossing(trace, 20.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
|
||||
time_to_50 = {profile: crossing(trace, 50.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
|
||||
first_motion = {profile: int(np.flatnonzero(trace[:, 1] > 0.01)[0]) for profile, trace in traces.items()}
|
||||
|
||||
self.assertEqual(len(set(first_motion.values())), 1)
|
||||
self.assertTrue(all(trace[0, 2] > 0.0 and trace[1, 3] > 0.0 for trace in traces.values()))
|
||||
self.assertLess(time_to_20[AccelProfile.eco], 8.0)
|
||||
self.assertLess(time_to_50[AccelProfile.eco], 27.0)
|
||||
self.assertGreaterEqual(time_to_20[AccelProfile.eco] - time_to_20[AccelProfile.normal], 0.5)
|
||||
self.assertGreaterEqual(time_to_20[AccelProfile.normal] - time_to_20[AccelProfile.sport], 0.5)
|
||||
self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 2.0)
|
||||
self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 3.0)
|
||||
|
||||
# Asserted against stock rather than against the actuator's rate limit. The peak command jerk in this run
|
||||
# is stock's stop-release ramp at launch (LongCtrlState.stopping -> pid), which on its own already exceeds
|
||||
# PRIUS_TSS2_ROUTE_MODEL.command_rate_limit: measured 4.392 for stock and for all three profiles alike.
|
||||
# An absolute bound here would only be testing that stock ramp, and would pass or fail on stock changes
|
||||
# that have nothing to do with the profiles. What this test can honestly own is that the profiles add no
|
||||
# command jerk of their own.
|
||||
stock_peak_jerk = float(np.max(np.abs(np.diff(stock[:, 3])) / DT_MDL))
|
||||
for profile, trace in traces.items():
|
||||
command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL
|
||||
self.assertLessEqual(float(np.max(command_jerk)), stock_peak_jerk + 1e-9, profile)
|
||||
|
||||
settled = np.flatnonzero(trace[:, 1] >= 25.0 - 0.15)
|
||||
self.assertGreater(len(settled), 0)
|
||||
settled_trace = trace[settled[0]:]
|
||||
self.assertGreaterEqual(float(np.min(settled_trace[:, 3])), -0.05)
|
||||
self.assertLessEqual(float(np.max(trace[:, 1])), 25.0 + 1e-9)
|
||||
|
||||
def test_blended_positive_model_request_uses_profile_cruise_cap(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_acceleration(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return 2.0, False
|
||||
|
||||
plant = PlantSP(speed=15.0, e2e=True, model_action_fn=request_acceleration)
|
||||
_set_mpc_acceleration(plant)
|
||||
results = [plant.step(v_cruise=35.0) for _ in range(20)]
|
||||
settled = results[-1]
|
||||
eco_limit = float(np.interp(settled["published_v_ego"], MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
|
||||
self.assertTrue(settled["controller_active"])
|
||||
self.assertEqual(settled["mpc_source"], LongitudinalPlanSource.cruise)
|
||||
self.assertAlmostEqual(settled["a_target"], eco_limit, delta=0.01)
|
||||
self.assertLess(settled["a_target"], settled["model_action"]["desiredAcceleration"])
|
||||
|
||||
def test_blended_profile_does_not_change_model_braking(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_braking(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return -0.8, False
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=20.0, e2e=True, model_action_fn=request_braking)
|
||||
_set_mpc_acceleration(plant)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
|
||||
self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.e2e for row in traces[True]))
|
||||
self.assertTrue(all(row["controller_active"] for row in traces[True]))
|
||||
self.assertTrue(all(not row["controller_active"] for row in traces[False]))
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
|
||||
|
||||
def test_profile_does_not_change_model_stop_request(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_stop(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return -0.8, True
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=1.0, e2e=True, model_action_fn=request_stop)
|
||||
_set_mpc_acceleration(plant)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
|
||||
|
||||
def test_profile_does_not_change_lead_braking(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=20.0)
|
||||
_set_mpc_acceleration(plant, -0.8)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
|
||||
self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.lead0 for row in traces[True]))
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
|
||||
|
||||
def test_normal_launch_is_faster_than_eco(self):
|
||||
eco = run_profile(AccelProfile.eco, speed=4.0, steps=120)
|
||||
normal = run_profile(AccelProfile.normal, speed=4.0, steps=120)
|
||||
self.assertGreater(normal[-1][0], eco[-1][0])
|
||||
|
||||
def test_profiles_do_not_change_far_braking(self):
|
||||
# The ceiling is an upper bound only, so a deceleration is bit-identical to stock for every profile.
|
||||
for e2e in (False, True):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.assertEqual(run_profile(profile, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100), stock)
|
||||
|
||||
def test_cruise_decel_is_identical_to_stock_for_every_profile(self):
|
||||
# Deceleration is deliberately NOT profile-dependent: the controller sets an acceleration ceiling and
|
||||
# nothing else, and an upper bound cannot participate in a brake. Stock's clip to A_CRUISE_MIN owns it.
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=25.0, v_cruise=20.0, steps=220)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.assertEqual(run_profile(profile, speed=25.0, v_cruise=20.0, steps=220), stock, profile)
|
||||
|
||||
def test_blended_launch_respects_profiles(self):
|
||||
traces = {
|
||||
profile: run_profile(profile, v_cruise=8.0, e2e=True, steps=180)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
time_to_five = {
|
||||
profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
|
||||
self.assertLess(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
|
||||
self.assertLess(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
|
||||
|
||||
def test_launch_ordering_without_departure_delay(self):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, v_cruise=8.0, steps=160)
|
||||
traces = {
|
||||
profile: run_profile(profile, v_cruise=8.0, steps=160)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
first_motion = {
|
||||
profile: next(frame for frame, row in enumerate(rows) if row[0] > 0.01)
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
time_to_five = {
|
||||
profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
stock_first_motion = next(frame for frame, row in enumerate(stock) if row[0] > 0.01)
|
||||
|
||||
# No launch dead time: motion starts on the same frame as stock, because the ceiling only ever bounds the
|
||||
# command from above and stock's own law owns the first frame.
|
||||
self.assertEqual(len(set(first_motion.values())), 1)
|
||||
self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values()))
|
||||
self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.1)
|
||||
self.assertGreaterEqual(time_to_five[AccelProfile.normal] - time_to_five[AccelProfile.sport], 0.1)
|
||||
|
||||
def test_road_speed_catchup_stays_useful(self):
|
||||
traces = {
|
||||
profile: run_profile(profile, speed=20.0, v_cruise=30.0, steps=100)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
gains = {profile: rows[-1][0] - 20.0 for profile, rows in traces.items()}
|
||||
self.assertGreater(gains[AccelProfile.normal] - gains[AccelProfile.eco], 0.05)
|
||||
self.assertGreater(gains[AccelProfile.sport] - gains[AccelProfile.normal], 0.1)
|
||||
|
||||
def test_full_catchup_trace_respects_stock_jerk(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
rows = run_profile(profile, v_cruise=30.0, steps=300)
|
||||
previous_speed = 0.0
|
||||
previous_accel = 0.0
|
||||
for speed, accel, _should_stop in rows:
|
||||
jerk_step = float(np.interp(previous_speed, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
|
||||
self.assertLessEqual(abs(accel - previous_accel), jerk_step + 1e-12)
|
||||
previous_speed = speed
|
||||
previous_accel = accel
|
||||
|
||||
def test_stop_release_frame_is_profile_independent(self):
|
||||
def target_speed(frame: int) -> float:
|
||||
return 0.0 if frame < 20 else 8.0
|
||||
|
||||
traces = {
|
||||
profile: run_profile(profile, v_cruise_fn=target_speed, steps=80)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
release_frames = {
|
||||
profile: next(frame for frame, row in enumerate(rows) if frame >= 20 and not row[2])
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, v_cruise_fn=target_speed, steps=80)
|
||||
stock_release_frame = next(frame for frame, row in enumerate(stock) if frame >= 20 and not row[2])
|
||||
self.assertEqual(len(set(release_frames.values())), 1)
|
||||
self.assertTrue(all(frame == stock_release_frame for frame in release_frames.values()))
|
||||
@@ -5,10 +5,13 @@ 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.cereal import messaging, custom
|
||||
import math
|
||||
|
||||
from openpilot.cereal import messaging, custom, log
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||
@@ -19,12 +22,16 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
|
||||
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
|
||||
|
||||
E2E_BRAKE_HOLD_ACCEL = -0.2 # m/s^2
|
||||
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
|
||||
self.accel_controller = AccelController()
|
||||
self.accel_controller_active = False
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
@@ -32,16 +39,56 @@ class LongitudinalPlannerSP:
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self.a_cruise = 0. # re-assigned by the subclass; declared here because get_max_accel_override clamps it
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
|
||||
def is_e2e(self, sm: messaging.SubMaster) -> bool:
|
||||
experimental_mode = sm['selfdriveState'].experimentalMode
|
||||
if not self.dec.active():
|
||||
return experimental_mode
|
||||
if not experimental_mode:
|
||||
return False
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
if not self.dec.active() or self.dec.mode() == "blended":
|
||||
return True
|
||||
|
||||
# hold a brake the model already owns rather than release it mid-brake; min() means this can only ever
|
||||
# add deceleration, and never one that was not already the selected source
|
||||
if self.mpc.source == MpcPlanSource.e2e and sm['modelV2'].action.desiredAcceleration < E2E_BRAKE_HOLD_ACCEL:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def get_max_accel_override(self, v_ego: float, _v_target: float, e2e: bool) -> float | None:
|
||||
"""Pure speed-scheduled authority. The arrival taper is the comfort law's job, not the ceiling's."""
|
||||
self.accel_controller_active = bool(self.accel_controller.is_enabled() and (e2e or self.allow_throttle))
|
||||
if not self.accel_controller_active:
|
||||
return None
|
||||
|
||||
ceiling = self.accel_controller.get_max_accel(v_ego)
|
||||
|
||||
# get_cruise_accel jerk-limits AFTER clipping to max_accel, so a carried value above the ceiling ratchets
|
||||
# the command back over it. upper side only: never make braking less negative
|
||||
if math.isfinite(self.a_cruise):
|
||||
self.a_cruise = min(self.a_cruise, ceiling)
|
||||
|
||||
return ceiling
|
||||
|
||||
def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
|
||||
radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
|
||||
return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
|
||||
(source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
|
||||
|
||||
def arbitrate_cruise_candidate(self, sm: messaging.SubMaster, gated: float, ungated: float,
|
||||
mpc_accel: float, mpc_source: MpcPlanSource, *, allow_throttle: bool,
|
||||
e2e: bool, force_decel: bool) -> float:
|
||||
finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel))
|
||||
coast_gate_changed_source = gated < mpc_accel <= ungated
|
||||
if (finite and not allow_throttle and not e2e and not force_decel
|
||||
and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source):
|
||||
return ungated
|
||||
|
||||
return gated
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
@@ -74,10 +121,13 @@ class LongitudinalPlannerSP:
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.accel_controller.update()
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
def update_dec(self, sm: messaging.SubMaster) -> None:
|
||||
self.dec.update(sm)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
|
||||
@@ -94,6 +144,15 @@ class LongitudinalPlannerSP:
|
||||
dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
dec.decelIntent = float(self.dec.signals.decel_intent)
|
||||
dec.curveDetected = bool(self.dec.signals.curve_detected)
|
||||
dec.wantBlended = bool(self.dec.want_blended)
|
||||
dec.leadVeto = bool(self.dec.lead_veto)
|
||||
|
||||
accel_controller = longitudinalPlanSP.accelController
|
||||
accel_controller.enabled = bool(self.accel_controller.is_enabled())
|
||||
accel_controller.active = bool(self.accel_controller_active)
|
||||
accel_controller.profile = int(self.accel_controller.profile)
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
|
||||
@@ -0,0 +1,433 @@
|
||||
"""
|
||||
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 collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import asdict, dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
|
||||
|
||||
|
||||
LeadObservation = dict[str, Any]
|
||||
LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
|
||||
ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
|
||||
EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
|
||||
ModelPlanFn = Callable[[float, float, float], list[float]]
|
||||
ModelMetaFn = Callable[[float], tuple[list[float], bool, float]]
|
||||
LeadFutureProbsFn = Callable[[float], tuple[float, float, float]]
|
||||
PositionYFn = Callable[[float], list[float]]
|
||||
ExperimentalModeFn = Callable[[float], bool]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ActuatorModel:
|
||||
planner_delay: float
|
||||
transport_delay: float
|
||||
actuator_lag: float
|
||||
command_rate_limit: float
|
||||
stopping_acceleration: float
|
||||
standstill_breakaway_acceleration: float
|
||||
standstill_breakaway_time: float
|
||||
|
||||
def __post_init__(self):
|
||||
nonnegative_fields = {
|
||||
"planner_delay": self.planner_delay,
|
||||
"transport_delay": self.transport_delay,
|
||||
"actuator_lag": self.actuator_lag,
|
||||
"standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
|
||||
"standstill_breakaway_time": self.standstill_breakaway_time,
|
||||
}
|
||||
if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
|
||||
raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
|
||||
if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
|
||||
raise ValueError("command_rate_limit must be finite and positive")
|
||||
if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
|
||||
raise ValueError("stopping_acceleration must be finite and non-positive")
|
||||
|
||||
|
||||
# Conservative Prius TSS2 actuator model.
|
||||
PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
|
||||
planner_delay=0.05,
|
||||
transport_delay=0.0,
|
||||
actuator_lag=0.20,
|
||||
command_rate_limit=4.0,
|
||||
stopping_acceleration=-2.0,
|
||||
standstill_breakaway_acceleration=1.0,
|
||||
standstill_breakaway_time=0.05,
|
||||
)
|
||||
|
||||
|
||||
class PlantSP(Plant):
|
||||
"""Closed-loop plant with configurable observations and actuator response."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
lead_relevancy=False,
|
||||
speed=0.0,
|
||||
distance_lead=2.0,
|
||||
enabled=True,
|
||||
only_lead2=False,
|
||||
only_radar=False,
|
||||
e2e=False,
|
||||
personality=0,
|
||||
force_decel=False,
|
||||
lead_observation_fn: LeadObservationFn | None = None,
|
||||
model_action_fn: ModelActionFn | None = None,
|
||||
ego_observation_fn: EgoObservationFn | None = None,
|
||||
model_plan_fn: ModelPlanFn | None = None,
|
||||
model_meta_fn: ModelMetaFn | None = None,
|
||||
lead_future_probs_fn: LeadFutureProbsFn | None = None,
|
||||
position_y_fn: PositionYFn | None = None,
|
||||
experimental_mode_fn: ExperimentalModeFn | None = None,
|
||||
actuator_delay: float | None = None,
|
||||
actuator_lag: float = 0.0,
|
||||
actuator_model: ActuatorModel | None = None,
|
||||
run_long_control: bool = False,
|
||||
):
|
||||
if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
|
||||
raise ValueError("actuator_delay must be finite and non-negative")
|
||||
if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
|
||||
raise ValueError("actuator_lag must be finite and non-negative")
|
||||
|
||||
self.rate = 1.0 / DT_MDL
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
Plant.radar = messaging.pub_sock('radarState')
|
||||
Plant.controls_state = messaging.pub_sock('controlsState')
|
||||
Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
|
||||
Plant.car_state = messaging.pub_sock('carState')
|
||||
Plant.plan = messaging.sub_sock('longitudinalPlan')
|
||||
Plant.messaging_initialized = True
|
||||
|
||||
self.v_lead_prev = 0.0
|
||||
|
||||
self.distance = 0.0
|
||||
self.speed = speed
|
||||
self.should_stop = False
|
||||
self.acceleration = 0.0
|
||||
self.a_target = 0.0
|
||||
self.actuator_command = 0.0
|
||||
self.applied_actuator_command = 0.0
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
# lead car
|
||||
self.lead_relevancy = lead_relevancy
|
||||
self.distance_lead = distance_lead
|
||||
self.enabled = enabled
|
||||
self.only_lead2 = only_lead2
|
||||
self.only_radar = only_radar
|
||||
self.e2e = e2e
|
||||
self.personality = personality
|
||||
self.force_decel = force_decel
|
||||
self.lead_observation_fn = lead_observation_fn
|
||||
self.model_action_fn = model_action_fn
|
||||
self.ego_observation_fn = ego_observation_fn
|
||||
self.model_plan_fn = model_plan_fn
|
||||
self.model_meta_fn = model_meta_fn
|
||||
self.lead_future_probs_fn = lead_future_probs_fn
|
||||
self.position_y_fn = position_y_fn
|
||||
self.experimental_mode_fn = experimental_mode_fn
|
||||
self.actuator_model = actuator_model
|
||||
self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
|
||||
self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
|
||||
self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
|
||||
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
|
||||
actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
|
||||
|
||||
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
|
||||
self.ts = 1.0 / self.rate
|
||||
time.sleep(0.1)
|
||||
self.sm = messaging.SubMaster(['longitudinalPlan'])
|
||||
|
||||
from opendbc.car.honda.values import CAR
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
if self.actuator_delay is not None:
|
||||
CP.longitudinalActuatorDelay = self.actuator_delay
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
|
||||
self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
|
||||
self.long_control = LongControl(CP, CP_SP) if run_long_control else None
|
||||
|
||||
if self.actuator_model is not None and self.speed >= 0.01:
|
||||
self.breakaway_confirmed = True
|
||||
self.integration_dt = DT_CTRL if run_long_control else self.ts
|
||||
delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
|
||||
self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
|
||||
|
||||
@staticmethod
|
||||
def _lead_message(observation: LeadObservation):
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
for field, value in observation.items():
|
||||
setattr(lead, field, value)
|
||||
return lead
|
||||
|
||||
def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
|
||||
if self.lead_observation_fn is None:
|
||||
return dict(truth) if present_by_default else None
|
||||
|
||||
observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
|
||||
if observed is None:
|
||||
return None
|
||||
|
||||
complete_observation = dict(truth)
|
||||
complete_observation.update(observed)
|
||||
return complete_observation
|
||||
|
||||
def _update_actuator(self, command: float) -> tuple[float, float]:
|
||||
if self._actuator_delay_queue:
|
||||
self._actuator_delay_queue.append(command)
|
||||
delayed_command = self._actuator_delay_queue.popleft()
|
||||
else:
|
||||
delayed_command = command
|
||||
|
||||
if self.actuator_model is not None:
|
||||
max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
|
||||
self.applied_actuator_command = float(np.clip(delayed_command,
|
||||
self.applied_actuator_command - max_command_delta,
|
||||
self.applied_actuator_command + max_command_delta))
|
||||
|
||||
if self.speed < 0.01:
|
||||
if self.applied_actuator_command <= 0.0:
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
elif not self.breakaway_confirmed:
|
||||
breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
|
||||
if breakaway_ready:
|
||||
self._breakaway_timer += self.integration_dt
|
||||
else:
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
|
||||
if not self.breakaway_confirmed:
|
||||
self.acceleration = 0.0
|
||||
return delayed_command, self.acceleration
|
||||
else:
|
||||
self.breakaway_confirmed = True
|
||||
|
||||
response_command = self.applied_actuator_command
|
||||
else:
|
||||
self.applied_actuator_command = delayed_command
|
||||
response_command = delayed_command
|
||||
|
||||
if self.actuator_lag > 0.0:
|
||||
alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
|
||||
self.acceleration += alpha * (response_command - self.acceleration)
|
||||
else:
|
||||
self.acceleration = response_command
|
||||
return delayed_command, self.acceleration
|
||||
|
||||
def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
|
||||
self.speed += self.acceleration * dt
|
||||
if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
|
||||
self.speed = self.acceleration = 0.0
|
||||
self.distance += self.speed * dt
|
||||
|
||||
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
radar = messaging.new_message('radarState')
|
||||
control = messaging.new_message('controlsState')
|
||||
ss = messaging.new_message('selfdriveState')
|
||||
car_state = messaging.new_message('carState')
|
||||
vehicle_parameters = messaging.new_message('vehicleParameters')
|
||||
car_control = messaging.new_message('carControl')
|
||||
model = messaging.new_message('modelV2')
|
||||
car_state_sp = messaging.new_message('carStateSP')
|
||||
live_map_data_sp = messaging.new_message('liveMapDataSP')
|
||||
gps_data = messaging.new_message('gpsLocation')
|
||||
a_lead = (v_lead - self.v_lead_prev) / self.ts
|
||||
self.v_lead_prev = v_lead
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
if self.only_radar:
|
||||
status = True
|
||||
elif prob_lead > 0.5:
|
||||
status = True
|
||||
else:
|
||||
status = False
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
prob_lead = 0.0
|
||||
status = False
|
||||
|
||||
truth_lead: LeadObservation = {
|
||||
"dRel": float(d_rel),
|
||||
"yRel": 0.0,
|
||||
"vRel": float(v_rel),
|
||||
"vLead": float(v_lead),
|
||||
"vLeadK": float(v_lead),
|
||||
"aLeadK": float(a_lead),
|
||||
"present": bool(status),
|
||||
# TODO use real radard logic for this
|
||||
"aLeadTau": float(_LEAD_ACCEL_TAU),
|
||||
"modelProb": float(prob_lead),
|
||||
"radar": bool(self.only_radar),
|
||||
"radarTrackId": -1,
|
||||
}
|
||||
lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
|
||||
lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
|
||||
if lead_one_observation is not None:
|
||||
radar.radarState.leadOne = self._lead_message(lead_one_observation)
|
||||
if lead_two_observation is not None:
|
||||
radar.radarState.leadTwo = self._lead_message(lead_two_observation)
|
||||
|
||||
# Simulate model predicting slightly faster speed
|
||||
# this is to ensure lead policy is effective when model
|
||||
# does not predict slowdown in e2e mode
|
||||
position = log.XYZTData.new_message()
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
if self.position_y_fn is None:
|
||||
position.y = [0.0] * len(ModelConstants.T_IDXS)
|
||||
else:
|
||||
position.y = [float(y) for y in self.position_y_fn(self.current_time)]
|
||||
model.modelV2.position = position
|
||||
if self.model_action_fn is None:
|
||||
model_acceleration, model_should_stop = self.acceleration + 0.5, False
|
||||
else:
|
||||
model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
|
||||
model.modelV2.action.desiredAcceleration = float(model_acceleration)
|
||||
model.modelV2.action.shouldStop = bool(model_should_stop)
|
||||
velocity = log.XYZTData.new_message()
|
||||
if self.model_plan_fn is None:
|
||||
velocity_plan = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity_plan[0] = float(self.speed) # always start at current speed
|
||||
else:
|
||||
velocity_plan = [float(x) for x in self.model_plan_fn(self.current_time, self.speed, self.acceleration)]
|
||||
velocity.x = velocity_plan
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
|
||||
if self.model_meta_fn is None:
|
||||
brake3_probs, hard_brake_predicted, frame_drop_perc = [0.0] * 5, False, 0.0
|
||||
else:
|
||||
brake3_probs, hard_brake_predicted, frame_drop_perc = self.model_meta_fn(self.current_time)
|
||||
model.modelV2.meta.disengagePredictions.brake3MetersPerSecondSquaredProbs = [float(p) for p in brake3_probs]
|
||||
model.modelV2.meta.hardBrakePredicted = bool(hard_brake_predicted)
|
||||
model.modelV2.frameDropPerc = float(frame_drop_perc)
|
||||
if self.lead_future_probs_fn is not None:
|
||||
model.modelV2.init('leadsV3', 3)
|
||||
lead_future_probs = self.lead_future_probs_fn(self.current_time)
|
||||
for i, (prob, prob_time) in enumerate(zip(lead_future_probs, (0.0, 2.0, 4.0), strict=True)):
|
||||
model.modelV2.leadsV3[i].prob = float(prob)
|
||||
model.modelV2.leadsV3[i].probTime = prob_time
|
||||
|
||||
control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
|
||||
LongCtrlState.pid if self.enabled else LongCtrlState.off)
|
||||
ss.selfdriveState.experimentalMode = self.e2e if self.experimental_mode_fn is None else bool(self.experimental_mode_fn(self.current_time))
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
true_v_ego = self.speed
|
||||
true_a_ego = self.acceleration
|
||||
published_v_ego = true_v_ego
|
||||
published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
|
||||
if self.ego_observation_fn is not None:
|
||||
published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
|
||||
car_state.carState.vEgo = float(published_v_ego)
|
||||
car_state.carState.aEgo = float(published_a_ego)
|
||||
car_state.carState.standstill = bool(self.speed < 0.01)
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = PlannerSM(self.rk.frame, {
|
||||
'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
'selfdriveState': ss.selfdriveState,
|
||||
'vehicleParameters': vehicle_parameters.vehicleParameters,
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation,
|
||||
})
|
||||
self.planner.update(sm)
|
||||
self.a_target = self.planner.output_a_target
|
||||
if self.long_control is None:
|
||||
self.actuator_command = self.a_target
|
||||
if self.planner.output_should_stop:
|
||||
stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
|
||||
self.actuator_command = min(stopping_acceleration, self.actuator_command)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(self.ts)
|
||||
else:
|
||||
for _ in range(round(self.ts / DT_CTRL)):
|
||||
car_state.carState.vEgo = self.speed
|
||||
car_state.carState.aEgo = self.acceleration
|
||||
car_state.carState.standstill = self.speed < 0.01
|
||||
self.actuator_command = self.long_control.update(
|
||||
self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
|
||||
)
|
||||
self._update_actuator(self.actuator_command)
|
||||
self._integrate_ego(DT_CTRL, stop_at_standstill=True)
|
||||
self.should_stop = self.planner.output_should_stop
|
||||
fcw = self.planner.fcw
|
||||
self.distance_lead = self.distance_lead + v_lead * self.ts
|
||||
|
||||
# *** radar model ***
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
|
||||
# print at 5hz
|
||||
# if (self.rk.frame % (self.rate // 5)) == 0:
|
||||
# print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s"
|
||||
# % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
|
||||
|
||||
# ******** update prevs ********
|
||||
self.rk.monitor_time()
|
||||
|
||||
return {
|
||||
"distance": self.distance,
|
||||
"speed": self.speed,
|
||||
"acceleration": self.acceleration,
|
||||
"realized_acceleration": self.acceleration,
|
||||
"a_target": self.a_target,
|
||||
"actuator_command": self.actuator_command,
|
||||
"published_a_ego": published_a_ego,
|
||||
"published_v_ego": published_v_ego,
|
||||
"should_stop": self.should_stop,
|
||||
"long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
|
||||
else control.controlsState.longControlState.raw),
|
||||
"distance_lead": self.distance_lead,
|
||||
"fcw": fcw,
|
||||
"mpc_source": self.planner.mpc.source,
|
||||
"dec_mode": self.planner.dec.mode(),
|
||||
"dec_want_blended": self.planner.dec.want_blended,
|
||||
"dec_signals": asdict(self.planner.dec.signals),
|
||||
"dec_lead_veto": self.planner.dec.lead_veto,
|
||||
"controller_active": self.planner.accel_controller_active,
|
||||
"model_action": {
|
||||
"desiredAcceleration": float(model_acceleration),
|
||||
"shouldStop": bool(model_should_stop),
|
||||
},
|
||||
"truth_lead": dict(truth_lead),
|
||||
"lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
|
||||
"lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
|
||||
}
|
||||
+186
@@ -0,0 +1,186 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ENTER_FRAMES, MIN_BLENDED_FRAMES
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS)
|
||||
|
||||
|
||||
def decel_plan(a):
|
||||
def fn(_current_time, speed, _acceleration):
|
||||
return [float(max(0.0, speed + a * t)) for t in T_IDXS]
|
||||
return fn
|
||||
|
||||
|
||||
def flat_plan():
|
||||
def fn(_current_time, speed, _acceleration):
|
||||
return [float(speed)] * len(T_IDXS)
|
||||
return fn
|
||||
|
||||
|
||||
def alternating_plan(a):
|
||||
def fn(current_time, speed, _acceleration):
|
||||
frame_a = a if round(current_time / DT_MDL) % 2 == 0 else 0.0
|
||||
return [float(max(0.0, speed + frame_a * t)) for t in T_IDXS]
|
||||
return fn
|
||||
|
||||
|
||||
def persistent_lead_probs(_current_time):
|
||||
return (1.0, 0.95, 0.9)
|
||||
|
||||
|
||||
def _run(plant, steps, v_lead=0.0, v_cruise=50.0):
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
return [plant.step(v_lead=v_lead, v_cruise=v_cruise) for _ in range(steps)], solver_failures
|
||||
|
||||
|
||||
def mode_changes(results):
|
||||
modes = [r["dec_mode"] for r in results]
|
||||
return sum(a != b for a, b in zip(modes, modes[1:], strict=False))
|
||||
|
||||
|
||||
class TestDecManeuvers(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.params = Params()
|
||||
self.params.put_bool("DynamicExperimentalControl", True, block=True)
|
||||
|
||||
def test_s1_lead_clears_with_underlying_slowdown_blends_quickly(self):
|
||||
clear_t = 1.0
|
||||
|
||||
def lead_obs(current_time, _lead_name, truth):
|
||||
return None if current_time >= clear_t else dict(truth)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
lead_observation_fn=lead_obs, model_plan_fn=decel_plan(-2.5),
|
||||
lead_future_probs_fn=persistent_lead_probs)
|
||||
clear_frame = round(clear_t / DT_MDL)
|
||||
results, _ = _run(plant, steps=clear_frame + ENTER_FRAMES + 5, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results[:clear_frame])
|
||||
assert all(r["dec_lead_veto"] for r in results[:clear_frame])
|
||||
post_clear = [r["dec_mode"] for r in results[clear_frame:clear_frame + ENTER_FRAMES + 2]]
|
||||
assert "blended" in post_clear
|
||||
|
||||
def test_s1b_lead_clears_with_no_underlying_slowdown_stays_acc(self):
|
||||
clear_t = 1.0
|
||||
|
||||
def lead_obs(current_time, _lead_name, truth):
|
||||
return None if current_time >= clear_t else dict(truth)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
lead_observation_fn=lead_obs, model_plan_fn=flat_plan(),
|
||||
lead_future_probs_fn=persistent_lead_probs)
|
||||
clear_frame = round(clear_t / DT_MDL)
|
||||
results, _ = _run(plant, steps=clear_frame + MIN_BLENDED_FRAMES, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s2_steady_highway_following_never_blends(self):
|
||||
v = 80.0 / 3.6
|
||||
plant = PlantSP(lead_relevancy=True, speed=v, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
|
||||
results, failures = _run(plant, steps=100, v_lead=v, v_cruise=v)
|
||||
|
||||
assert failures <= 1
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s3_low_speed_cruise_no_lead_never_blends(self):
|
||||
v = 15.0 / 3.6
|
||||
plant = PlantSP(lead_relevancy=False, speed=v, e2e=True, model_plan_fn=flat_plan())
|
||||
results, _ = _run(plant, steps=100, v_cruise=v)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s4_highway_slowdown_without_lead_blends(self):
|
||||
v0 = 110.0 / 3.6
|
||||
a = (70.0 / 3.6 - v0) / 6.0
|
||||
plant = PlantSP(lead_relevancy=False, speed=v0, e2e=True, model_plan_fn=decel_plan(a))
|
||||
results, _ = _run(plant, steps=10, v_cruise=v0)
|
||||
|
||||
assert any(r["dec_mode"] == "blended" for r in results)
|
||||
|
||||
def test_s5_stop_then_depart_with_lead_present_stays_acc_throughout(self):
|
||||
def departing_lead(current_time):
|
||||
return 0.0 if current_time < 1.0 else min(15.0, 3.0 * (current_time - 1.0))
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=0.0, distance_lead=6.0, e2e=True)
|
||||
results = []
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
for _ in range(200):
|
||||
results.append(plant.step(v_lead=departing_lead(plant.current_time), v_cruise=15.0))
|
||||
|
||||
assert solver_failures <= 1
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
assert all(r["dec_lead_veto"] for r in results)
|
||||
|
||||
def test_s6_creep_cycles_behind_lead_stay_acc(self):
|
||||
def creep_cycle_lead(current_time):
|
||||
return 1.5 + 1.5 * np.sin(current_time * 2.0)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=1.0, distance_lead=8.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
|
||||
results = [plant.step(v_lead=creep_cycle_lead(plant.current_time), v_cruise=5.0) for _ in range(200)]
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s7_oscillating_near_threshold_demand_does_not_flap(self):
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=alternating_plan(-2.5))
|
||||
results, _ = _run(plant, steps=200, v_cruise=20.0)
|
||||
|
||||
assert mode_changes(results) <= 2
|
||||
|
||||
def test_s8_degraded_model_holds_acc_through_a_slowdown(self):
|
||||
def degraded_meta(_current_time):
|
||||
return [0.0] * 5, False, 60.0
|
||||
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-3.0), model_meta_fn=degraded_meta)
|
||||
results, _ = _run(plant, steps=30, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self):
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-1.0),
|
||||
position_y_fn=lambda _t: [6.0] * len(T_IDXS))
|
||||
results, _ = _run(plant, steps=30, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s11_curve_does_not_interrupt_an_active_hard_stop(self):
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
|
||||
position_y_fn=lambda _t: [6.0] * len(T_IDXS))
|
||||
results, _ = _run(plant, steps=10, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "blended" for r in results[ENTER_FRAMES - 1:])
|
||||
|
||||
def test_s10_hard_brake_override_inert_while_lead_present(self):
|
||||
def hard_brake_meta(_current_time):
|
||||
return [0.0] * 5, True, 0.0
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), model_meta_fn=hard_brake_meta, lead_future_probs_fn=persistent_lead_probs)
|
||||
results, _ = _run(plant, steps=10, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
@@ -0,0 +1,164 @@
|
||||
from collections.abc import Callable
|
||||
import math
|
||||
from typing import cast
|
||||
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
|
||||
|
||||
|
||||
def departing_lead(current_time: float) -> float:
|
||||
return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
|
||||
|
||||
|
||||
def stopped_lead(_current_time: float) -> float:
|
||||
return 0.0
|
||||
|
||||
|
||||
PARITY_SCENARIOS = {
|
||||
"approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
|
||||
"stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
|
||||
}
|
||||
|
||||
|
||||
def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
|
||||
plant = cls(**kwargs)
|
||||
plant.v_lead_prev = v_lead(0.0)
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
results = []
|
||||
for _ in range(steps):
|
||||
lead_speed = v_lead(plant.current_time)
|
||||
result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
|
||||
results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
|
||||
return results, solver_failures
|
||||
|
||||
|
||||
class TestPlantSP(OpenpilotTestCase):
|
||||
@parameterized.expand(PARITY_SCENARIOS, names=("scenario",), ids=lambda scenario: scenario)
|
||||
def test_plant_sp_matches_stock_plant_on_shared_kwargs(self, scenario: str):
|
||||
kwargs = dict(PARITY_SCENARIOS[scenario])
|
||||
v_cruise = cast(float, kwargs.pop("v_cruise"))
|
||||
v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
|
||||
steps = cast(int, kwargs.pop("steps"))
|
||||
|
||||
stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
|
||||
assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
|
||||
assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
|
||||
|
||||
for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
|
||||
zip(stock_results, sp_results, strict=True),
|
||||
):
|
||||
for key in STOCK_STEP_KEYS:
|
||||
if isinstance(stock_result[key], float):
|
||||
self.assertAlmostEqual(sp_result[key], stock_result[key], msg=f"{scenario} frame {frame} key {key}")
|
||||
else:
|
||||
assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
|
||||
assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
|
||||
self.assertAlmostEqual(sp_a_target, stock_a_target, msg=f"{scenario} frame {frame} output_a_target")
|
||||
|
||||
if scenario == "stop_then_depart":
|
||||
departure_frame = round(1.0 / DT_MDL)
|
||||
for results in (stock_results, sp_results):
|
||||
assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
|
||||
assert results[departure_frame - 1][0]["should_stop"]
|
||||
assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
|
||||
assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
|
||||
stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and not result["should_stop"])
|
||||
stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results)
|
||||
if frame >= departure_frame and result["speed"] > 0.05)
|
||||
assert sp_release == stock_release
|
||||
assert sp_motion == stock_motion
|
||||
|
||||
def test_full_lead_observation_is_independent_from_truth(self):
|
||||
callback_inputs = []
|
||||
|
||||
def observe_lead(current_time, lead_name, truth):
|
||||
callback_inputs.append((current_time, lead_name, truth))
|
||||
if lead_name == "leadOne":
|
||||
return {
|
||||
"dRel": 12.5,
|
||||
"vRel": -4.0,
|
||||
"vLead": 6.0,
|
||||
"vLeadK": 5.5,
|
||||
"aLeadK": -1.25,
|
||||
"aLeadTau": 0.7,
|
||||
"present": True,
|
||||
"modelProb": 0.9,
|
||||
"radarTrackId": 42,
|
||||
}
|
||||
return None
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
|
||||
result = plant.step(v_lead=8.0)
|
||||
|
||||
assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
|
||||
self.assertAlmostEqual(callback_inputs[0][2]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["truth_lead"]["dRel"], 50.0)
|
||||
self.assertAlmostEqual(result["lead_one_observation"]["dRel"], 12.5)
|
||||
assert result["lead_one_observation"]["radarTrackId"] == 42
|
||||
assert result["lead_two_observation"] is None
|
||||
self.assertAlmostEqual(result["distance_lead"], 50.0 + 8.0 * DT_MDL)
|
||||
|
||||
def test_model_action_realized_acceleration_and_source_logging(self):
|
||||
def model_action(current_time, v_ego, a_ego):
|
||||
return -1.25, True
|
||||
|
||||
plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
|
||||
first = plant.step()
|
||||
second = plant.step()
|
||||
|
||||
assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
|
||||
self.assertAlmostEqual(first["published_a_ego"], 0.0)
|
||||
self.assertAlmostEqual(second["published_a_ego"], first["realized_acceleration"])
|
||||
assert first["acceleration"] == first["realized_acceleration"]
|
||||
assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
|
||||
assert first["mpc_source"] is not None
|
||||
assert first["dec_mode"] in ("acc", "blended")
|
||||
assert "controller_active" in first
|
||||
assert first["lead_one_observation"] is not None
|
||||
assert first["truth_lead"] == first["lead_one_observation"]
|
||||
|
||||
def test_default_model_action_matches_stock_plant(self):
|
||||
result = PlantSP(speed=10.0).step()
|
||||
|
||||
self.assertAlmostEqual(result["model_action"]["desiredAcceleration"], 0.5)
|
||||
assert not result["model_action"]["shouldStop"]
|
||||
|
||||
def test_configurable_transport_delay_and_first_order_lag(self):
|
||||
plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
|
||||
|
||||
self.assertAlmostEqual(plant.planner.CP.longitudinalActuatorDelay, 2 * DT_MDL)
|
||||
delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
|
||||
assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
|
||||
|
||||
expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
|
||||
assert delayed_commands[2][0] == -1.0
|
||||
self.assertAlmostEqual(delayed_commands[2][1], expected_acceleration)
|
||||
|
||||
@parameterized.expand(
|
||||
[(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
|
||||
names=("delay", "lag"),
|
||||
)
|
||||
def test_invalid_actuator_dynamics(self, delay, lag):
|
||||
with self.assertRaises(ValueError):
|
||||
PlantSP(actuator_delay=delay, actuator_lag=lag)
|
||||
@@ -652,6 +652,53 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Eco"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Sport"
|
||||
}
|
||||
],
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -43,6 +43,28 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Enable Accel Controller
|
||||
description: Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are
|
||||
unchanged.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.
|
||||
options:
|
||||
- value: 0
|
||||
label: Eco
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 2
|
||||
label: Sport
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -276,6 +276,29 @@ class TestKnownPanels(OpenpilotTestCase):
|
||||
enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
|
||||
assert "NeuralNetworkLateralControl" in enhanced_enable_keys
|
||||
|
||||
def test_accel_controller_profile_mapping_and_enablement(self, schema):
|
||||
cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
|
||||
items = {item["key"]: item for item in _iter_panel_items(cruise)}
|
||||
|
||||
assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
|
||||
assert items["AccelPersonality"]["options"] == [
|
||||
{"value": 0, "label": "Eco"},
|
||||
{"value": 1, "label": "Normal"},
|
||||
{"value": 2, "label": "Sport"},
|
||||
]
|
||||
assert {
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": True,
|
||||
} in items["AccelPersonalityEnabled"]["enablement"]
|
||||
assert {
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": True,
|
||||
} in items["AccelPersonality"]["enablement"]
|
||||
profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"}
|
||||
assert "AccelPersonalityEnabled" not in profile_enable_keys
|
||||
|
||||
|
||||
class TestKnownVehicleSettings(OpenpilotTestCase):
|
||||
def test_hyundai_has_longitudinal_tuning(self, schema):
|
||||
|
||||
Reference in New Issue
Block a user