From eb46928db04b25b84d4fbb6f0d8bf379132ee21d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 02:47:30 -0400 Subject: [PATCH] sync with latest --- cereal/custom.capnp | 1 + .../controls/lib/longitudinal_planner.py | 21 ++- .../controls/lib/speed_limit/common.py | 7 + .../speed_limit_assist.py | 64 ++++---- .../tests/test_speed_limit_assist.py | 83 ++++------- .../lib/speed_limit_assist/__init__.py | 20 --- .../controls/lib/speed_limit_assist/common.py | 15 -- .../speed_limit_resolver.py | 133 ----------------- .../lib/speed_limit_assist/tests/__init__.py | 0 .../tests/test_speed_limit_resolver.py | 138 ------------------ 10 files changed, 77 insertions(+), 405 deletions(-) rename sunnypilot/selfdrive/controls/lib/{speed_limit_assist => speed_limit}/speed_limit_assist.py (82%) rename sunnypilot/selfdrive/controls/lib/{speed_limit_assist => speed_limit}/tests/test_speed_limit_assist.py (59%) delete mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py delete mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py delete mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b4882b4203..9322df97a0 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -219,6 +219,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { enum LongitudinalPlanSource { cruise @0; sccVision @1; + speedLimitAssist @2; } } diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 492c08ac9b..97d75d29ac 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -9,13 +9,13 @@ from cereal import messaging, custom from opendbc.car import structs from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState -Source = custom.LongitudinalPlanSP.LongitudinalPlanSource +LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource class LongitudinalPlannerSP: @@ -29,7 +29,7 @@ class LongitudinalPlannerSP: self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None - self.source = Source.cruise + self.source = LongitudinalPlanSource.cruise @property def mlsim(self) -> bool: @@ -50,18 +50,17 @@ class LongitudinalPlannerSP: self.scc.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise) - # Speed Limit Assist - self.resolver.update(v_ego, sm) - v_cruise_sla = self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster, - self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp) - # Speed Limit Resolver self.resolver.update(v_ego, sm) + # Speed Limit Assist + self.sla.update(long_enabled, long_override, v_ego, a_ego, sm['carState'].vCruiseCluster, + self.resolver.speed_limit, self.resolver.speed_limit_offset, self.resolver.distance, self.events_sp) + targets = { - Source.cruise: (v_cruise, a_ego), - Source.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), - Source.speedLimitAssist: (v_cruise_sla, a_ego), + LongitudinalPlanSource.cruise: (v_cruise, a_ego), + LongitudinalPlanSource.sccVision: (self.scc.vision.output_v_target, self.scc.vision.output_a_target), + LongitudinalPlanSource.speedLimitAssist: (self.sla.output_a_target, a_ego), } self.source = min(targets, key=lambda k: targets[k][0]) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py index 7d219ac3bb..baf9328032 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/common.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/common.py @@ -19,3 +19,10 @@ class OffsetType(IntEnum): off = 0 fixed = 1 percentage = 2 + + +class Mode(IntEnum): + off = 0 + information = 1 + warning = 2 + assist = 3 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py similarity index 82% rename from sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py rename to sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index a8661b8575..5fce4a54fc 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -7,32 +7,46 @@ See the LICENSE.md file in the root directory for more details. import numpy as np from cereal import custom -from openpilot.common.constants import CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \ - SpeedLimitAssistState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD +from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode from openpilot.selfdrive.modeld.constants import ModelConstants EventNameSP = custom.OnroadEventSP.EventName -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting) ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, SpeedLimitAssistState.overriding, *ACTIVE_STATES) +DISABLED_GUARD_PERIOD = 2 # secs. +PRE_ACTIVE_GUARD_PERIOD = 5 # secs. Time to wait after activation before considering temp deactivation signal. + +LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. +LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. +LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. +LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. + +# Speed Limit Assist Auto mode constants +REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params +CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial +FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available + class SpeedLimitAssist: _speed_limit: float + _speed_limit_offset: float _distance: float - _source: custom.LongitudinalPlanSP.SpeedLimitSource v_ego: float a_ego: float v_offset: float last_valid_speed_limit_final: float + output_v_target: float + output_a_target: float def __init__(self, CP): self.params = Params() @@ -41,7 +55,7 @@ class SpeedLimitAssist: self.long_engaged_timer = 0 self.pre_active_timer = 0 self.is_metric = self.params.get_bool("IsMetric") - self.enabled = self.params.get_bool("SpeedLimitAssist") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist self.long_enabled = False self.long_enabled_prev = False self.long_override = False @@ -54,17 +68,14 @@ class SpeedLimitAssist: self.v_cruise_setpoint_prev = 0. self.initial_max_set = False self._speed_limit = 0. + self._speed_limit_offset = 0. self.speed_limit_prev = 0. self.last_valid_speed_limit_final = 0. self._distance = 0. - self._source = SpeedLimitSource.none self.state = SpeedLimitAssistState.disabled self._state_prev = SpeedLimitAssistState.disabled self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True)) - self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) - # Solution functions mapped to respective states self.acceleration_solutions = { SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target, @@ -77,16 +88,12 @@ class SpeedLimitAssist: @property def speed_limit_final(self) -> float: - return self._speed_limit + self.speed_limit_offset + return self._speed_limit + self._speed_limit_offset @property def speed_limit_changed(self) -> bool: return bool(self._speed_limit != self.speed_limit_prev) - @property - def speed_limit_offset(self) -> float: - return self.get_offset(self.offset_type, self.offset_value) - @property def v_cruise_setpoint_changed(self) -> bool: return bool(self.v_cruise_setpoint != self.v_cruise_setpoint_prev) @@ -105,22 +112,10 @@ class SpeedLimitAssist: # Fallback return V_CRUISE_UNSET - def get_offset(self, offset_type: OffsetType, offset_value: int) -> float: - if offset_type == OffsetType.off: - return 0 - elif offset_type == OffsetType.fixed: - return offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) - elif offset_type == OffsetType.percentage: - return offset_value * 0.01 * self._speed_limit - else: - raise NotImplementedError("Offset not supported") - def update_params(self) -> None: if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.enabled = self.params.get_bool("SpeedLimitAssist") - self.offset_type = OffsetType(self.params.get("SpeedLimitOffsetType", return_default=True)) - self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True) self.is_metric = self.params.get_bool("IsMetric") + self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist def initial_max_set_confirmed(self) -> bool: return bool(abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE) @@ -241,15 +236,15 @@ class SpeedLimitAssist: events_sp.add(EventNameSP.speedLimitChanged) def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float, - speed_limit: float, distance: float, source: custom.LongitudinalPlanSP.SpeedLimitSource, events_sp: EventsSP) -> float: + speed_limit: float, speed_limit_offset: float, distance: float, events_sp: EventsSP) -> None: self.long_enabled = long_enabled self.long_override = long_override self.v_ego = v_ego self.a_ego = a_ego self._speed_limit = speed_limit + self._speed_limit_offset = speed_limit_offset self._distance = distance - self._source = source self.update_params() self.update_calculations(v_cruise_setpoint) @@ -260,8 +255,7 @@ class SpeedLimitAssist: self.speed_limit_prev = self._speed_limit self.v_cruise_setpoint_prev = self.v_cruise_setpoint self.long_enabled_prev = self.long_enabled + + self.output_v_target = self.get_v_target_from_control() + self.frame += 1 - - v_target = self.get_v_target_from_control() - - return v_target diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py similarity index 59% rename from sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py rename to sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 904493a2ba..c4197f6505 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -4,8 +4,6 @@ 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 pytest - from cereal import custom from opendbc.car.car_helpers import interfaces @@ -15,13 +13,12 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import OffsetType -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import SpeedLimitAssistState, REQUIRED_INITIAL_MAX_SET_SPEED, \ - PRE_ACTIVE_GUARD_PERIOD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_assist import SpeedLimitAssist, ACTIVE_STATES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \ + REQUIRED_INITIAL_MAX_SET_SPEED, PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource +SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values()) @@ -55,7 +52,7 @@ class TestSpeedLimitAssist: return CI def reset_custom_params(self): - self.params.put_bool("SpeedLimitAssist", True) + self.params.put("SpeedLimitMode", int(Mode.assist)) self.params.put_bool("IsMetric", False) self.params.put("SpeedLimitOffsetType", 0) self.params.put("SpeedLimitValueOffset", 0) @@ -71,7 +68,6 @@ class TestSpeedLimitAssist: self.sla.speed_limit_prev = 0. self.sla.last_valid_speed_limit_offsetted = 0. self.sla._distance = 0. - self.sla._source = SpeedLimitSource.none self.sla.v_cruise_setpoint = 0. self.sla.v_cruise_setpoint_prev = 0. self.events_sp.clear() @@ -88,60 +84,60 @@ class TestSpeedLimitAssist: assert V_CRUISE_UNSET == self.sla.get_v_target_from_control() def test_disabled(self): - self.params.put_bool("SpeedLimitAssist", False) + self.params.put("SpeedLimitMode", int(Mode.off)) for _ in range(int(10. / DT_MDL)): - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.disabled def test_transition_disabled_to_preactive(self): for _ in range(int(3. / DT_MDL)): - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.preActive assert self.sla.is_enabled and not self.sla.is_active def test_preactive_to_active_with_max_speed_confirmation(self): self.sla.state = SpeedLimitAssistState.preActive - v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}" - assert v_cruise_sla == SPEED_LIMITS['city'] + assert self.sla.output_v_target == SPEED_LIMITS['city'] def test_preactive_timeout_to_inactive(self): self.sla.state = SpeedLimitAssistState.preActive - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp) for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.inactive def test_preactive_to_pending_no_speed_limit(self): self.sla.state = SpeedLimitAssistState.preActive - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.pending assert self.sla.is_enabled and not self.sla.is_active def test_pending_to_active_when_speed_limit_available(self): self.sla.state = SpeedLimitAssistState.pending - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active def test_pending_to_adapting_when_below_speed_limit(self): self.sla.state = SpeedLimitAssistState.pending - _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting assert self.sla.is_enabled and self.sla.is_active def test_active_to_adapting_transition(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - _ = self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting def test_adapting_to_active_transition(self): self.sla.state = SpeedLimitAssistState.adapting self.sla.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.active def test_manual_cruise_change_detection(self): @@ -150,27 +146,15 @@ class TestSpeedLimitAssist: self.sla.v_cruise_setpoint_prev = expected_cruise different_cruise = SPEED_LIMITS['highway'] + 5 - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.inactive - @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [ - (OffsetType.fixed, 5, SPEED_LIMITS['city'], 5 * CV.MPH_TO_MS), # 5 MPH fixed offset - (OffsetType.percentage, 10, SPEED_LIMITS['city'], 0.1 * SPEED_LIMITS['city']), # 10% offset - (OffsetType.off, 0, SPEED_LIMITS['city'], 0), # Off - (OffsetType.fixed, 10, SPEED_LIMITS['highway'], 10 * CV.MPH_TO_MS), # Different speed, fixed offset - (OffsetType.percentage, 5, SPEED_LIMITS['highway'], 0.05 * SPEED_LIMITS['highway']), # Different speed, percentage - ]) - def test_offset_calculations(self, offset_type, offset_value, speed_limit, expected_offset): - self.sla._speed_limit = speed_limit - actual_offset = self.sla.get_offset(offset_type, offset_value) - assert actual_offset == pytest.approx(expected_offset, rel=0.01) - def test_rapid_speed_limit_changes(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']] for _, speed_limit in enumerate(speed_limits): - _ = self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, 0, self.events_sp) assert self.sla.state in ACTIVE_STATES def test_invalid_speed_limits_handling(self): @@ -180,25 +164,18 @@ class TestSpeedLimitAssist: invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] for invalid_limit in invalid_limits: - v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp) - assert isinstance(v_cruise_sla, (int, float)) - assert v_cruise_sla == V_CRUISE_UNSET or v_cruise_sla > 0 + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, 0, self.events_sp) + assert isinstance(self.sla.output_v_target, (int, float)) + assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0 def test_stale_data_handling(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) old_speed_limit = SPEED_LIMITS['city'] self.sla.last_valid_speed_limit_final = old_speed_limit - v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, 0, self.events_sp) assert self.sla.state in ACTIVE_STATES - assert v_cruise_sla == old_speed_limit - - def test_different_speed_limit_sources(self): - self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - - for source in (SpeedLimitSource.car, SpeedLimitSource.map): - v_cruise_sla = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp) - assert v_cruise_sla != V_CRUISE_UNSET + assert self.sla.output_v_target == old_speed_limit def test_distance_based_adapting(self): self.sla.state = SpeedLimitAssistState.adapting @@ -208,17 +185,17 @@ class TestSpeedLimitAssist: current_speed = SPEED_LIMITS['highway'] target_speed = SPEED_LIMITS['city'] - v_cruise_sla = self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp) + self.sla.update(True, False, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, 0, distance, self.events_sp) assert self.sla.state == SpeedLimitAssistState.adapting - assert v_cruise_sla == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions + assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions def test_long_disengaged_to_disabled(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - v_cruise_sla = self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], - 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(False, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], + 0, 0, self.events_sp) assert self.sla.state == SpeedLimitAssistState.disabled - assert v_cruise_sla == V_CRUISE_UNSET + assert self.sla.output_v_target == V_CRUISE_UNSET def test_maintain_states_with_no_changes(self): """Test that states are maintained when no significant changes occur""" @@ -237,7 +214,7 @@ class TestSpeedLimitAssist: initial_state = state - _ = self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) + self.sla.update(True, False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, 0, self.events_sp) assert self.sla.state in ALL_STATES # Sanity check diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py deleted file mode 100644 index 2ac5b6a6f0..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/__init__.py +++ /dev/null @@ -1,20 +0,0 @@ -from cereal import custom - -SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimitAssistState - -PARAMS_UPDATE_PERIOD = 3. # secs. Time between parameter updates. -DISABLED_GUARD_PERIOD = 2 # secs. -PRE_ACTIVE_GUARD_PERIOD = 5 # secs. Time to wait after activation before considering temp deactivation signal. - -# Constants for Limit controllers. -LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. -LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. -LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active. -LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers. -LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state. -LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. - -# Speed Limit Assist Auto mode constants -REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params -CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial -FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py deleted file mode 100644 index 15e8ab8ad2..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/common.py +++ /dev/null @@ -1,15 +0,0 @@ -from enum import IntEnum - - -class Policy(IntEnum): - map_data_only = 0 - car_state_only = 1 - map_data_priority = 2 - car_state_priority = 3 - combined = 4 - - -class OffsetType(IntEnum): - off = 0 - fixed = 1 - percentage = 2 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py deleted file mode 100644 index 719abeec9b..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/speed_limit_resolver.py +++ /dev/null @@ -1,133 +0,0 @@ -import time -import numpy as np - -import cereal.messaging as messaging -from cereal import custom -from openpilot.common.gps import get_gps_location_service -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy - -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource - -ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values()) - - -class SpeedLimitResolver: - _limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float] - _distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimitSource, float] - _v_ego: float - speed_limit: float - distance: float - source: custom.LongitudinalPlanSP.SpeedLimitSource - - def __init__(self): - self.params = Params() - self.frame = -1 - - self._gps_location_service = get_gps_location_service(self.params) - self._limit_solutions = {} # Store for speed limit solutions from different sources - self._distance_solutions = {} # Store for distance to current speed limit start for different sources - - self.policy = self.params.get("SpeedLimitAssistPolicy", return_default=True) - self._policy_to_sources_map = { - Policy.car_state_only: [SpeedLimitSource.car], - Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], - Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car], - Policy.map_data_only: [SpeedLimitSource.map], - Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map], - } - self.source = SpeedLimitSource.none - for source in ALL_SOURCES: - self._reset_limit_sources(source) - - def update_params(self): - if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: - self.policy = Policy(self.params.get("SpeedLimitAssistPolicy", return_default=True)) - self.change_policy(self.policy) - - def change_policy(self, policy: Policy) -> None: - self.policy = policy - - def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimitSource) -> None: - self._limit_solutions[source] = 0. - self._distance_solutions[source] = 0. - - def _resolve_limit_sources(self, sm: messaging.SubMaster) -> None: - """Get limit solutions from each data source""" - self._get_from_car_state(sm) - self._get_from_map_data(sm) - - def _get_from_car_state(self, sm: messaging.SubMaster) -> None: - self._reset_limit_sources(SpeedLimitSource.car) - self._limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit - self._distance_solutions[SpeedLimitSource.car] = 0. - - def _get_from_map_data(self, sm: messaging.SubMaster) -> None: - self._reset_limit_sources(SpeedLimitSource.map) - self._process_map_data(sm) - - def _process_map_data(self, sm: messaging.SubMaster) -> None: - gps_data = sm[self._gps_location_service] - map_data = sm['liveMapDataSP'] - - gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3 - if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE: - return - - speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0. - next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0. - - self._calculate_map_data_limits(sm, speed_limit, next_speed_limit) - - def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None: - gps_data = sm[self._gps_location_service] - map_data = sm['liveMapDataSP'] - - distance_since_fix = self._v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3) - distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix) - - self._limit_solutions[SpeedLimitSource.map] = speed_limit - self._distance_solutions[SpeedLimitSource.map] = 0. - - if 0. < next_speed_limit < self._v_ego: - adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC - adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2 - - if distance_to_speed_limit_ahead <= adapt_distance: - self._limit_solutions[SpeedLimitSource.map] = next_speed_limit - self._distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead - - def _consolidate(self) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimitSource]: - source = self._get_source_solution_according_to_policy() - speed_limit = self._limit_solutions[source] if source else 0. - distance = self._distance_solutions[source] if source else 0. - - return speed_limit, distance, source - - def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimitSource: - sources_for_policy = self._policy_to_sources_map[self.policy] - - if self.policy != Policy.combined: - # They are ordered in the order of preference, so we pick the first that's non zero - for source in sources_for_policy: - return source if self._limit_solutions[source] > 0. else SpeedLimitSource.none - - limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float) - sources = np.array(sources_for_policy, dtype=int) - - if len(limits) > 0: - min_idx = np.argmin(limits) - return SpeedLimitSource(int(sources[min_idx])) - - return SpeedLimitSource.none - - def update(self, v_ego: float, sm: messaging.SubMaster) -> None: - self._v_ego = v_ego - self.update_params() - self._resolve_limit_sources(sm) - - self.speed_limit, self.distance, self.source = self._consolidate() - - self.frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py deleted file mode 100644 index 14433dc293..0000000000 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_assist/tests/test_speed_limit_resolver.py +++ /dev/null @@ -1,138 +0,0 @@ -import random -import time - -import pytest -from pytest_mock import MockerFixture - -from cereal import custom -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist import LIMIT_MAX_MAP_DATA_AGE - -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_assist.common import Policy - -SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource - - -def create_mock(properties, mocker: MockerFixture): - mock = mocker.MagicMock() - for _property, value in properties.items(): - setattr(mock, _property, value) - return mock - - -def setup_sm_mock(mocker: MockerFixture): - cruise_speed_limit = random.uniform(0, 120) - live_map_data_limit = random.uniform(0, 120) - - car_state = create_mock({ - 'gasPressed': False, - 'brakePressed': False, - 'standstill': False, - }, mocker) - car_state_sp = create_mock({ - 'speedLimit': cruise_speed_limit, - }, mocker) - live_map_data = create_mock({ - 'speedLimit': live_map_data_limit, - 'speedLimitValid': True, - 'speedLimitAhead': 0., - 'speedLimitAheadValid': 0., - 'speedLimitAheadDistance': 0., - }, mocker) - gps_data = create_mock({ - 'unixTimestampMillis': time.monotonic() * 1e3, - }, mocker) - sm_mock = mocker.MagicMock() - sm_mock.__getitem__.side_effect = lambda key: { - 'carState': car_state, - 'liveMapDataSP': live_map_data, - 'carStateSP': car_state_sp, - 'gpsLocation': gps_data, - }[key] - return sm_mock - - -parametrized_policies = pytest.mark.parametrize( - "policy, sm_key, function_key", [ - (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car), - (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car), - (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map), - (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map), - ], - ids=lambda val: val.name if hasattr(val, 'name') else str(val) -) - - -@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver]) -class TestSpeedLimitResolverValidation: - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_initial_state(self, resolver_class, policy): - resolver = resolver_class() - resolver.policy = policy - for source in ALL_SOURCES: - if source in resolver._limit_solutions: - assert resolver._limit_solutions[source] == 0. - assert resolver._distance_solutions[source] == 0. - - @parametrized_policies - def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the resolver - resolver.update(source_speed_limit, sm_mock) - assert resolver.speed_limit == source_speed_limit - assert resolver.source == ALL_SOURCES[function_key] - - def test_resolver_combined(self, resolver_class, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = Policy.combined - sm_mock = setup_sm_mock(mocker) - socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map} - minimum_key, minimum_speed_limit = min( - ((key, sm_mock[key].speedLimit) for key in - socket_to_source.keys()), key=lambda x: x[1]) - - # Assert the resolver - resolver.update(minimum_speed_limit, sm_mock) - assert resolver.speed_limit == minimum_speed_limit - assert resolver.source == socket_to_source[minimum_key] - - @parametrized_policies - def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = setup_sm_mock(mocker) - source_speed_limit = sm_mock[sm_key].speedLimit - - # Assert the parsing - resolver.update(source_speed_limit, sm_mock) - assert resolver._limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit - assert resolver._distance_solutions[ALL_SOURCES[function_key]] == 0. - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture): - v_ego = 50 - resolver = resolver_class() - resolver.policy = policy - - sm_mock = setup_sm_mock(mocker) - resolver.update(v_ego, sm_mock) - - # After resolution - assert resolver.speed_limit is not None - assert resolver.distance is not None - assert resolver.source is not None - - @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) - def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): - resolver = resolver_class() - resolver.policy = policy - sm_mock = mocker.MagicMock() - sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 - resolver._get_from_map_data(sm_mock) - assert resolver._limit_solutions[SpeedLimitSource.map] == 0. - assert resolver._distance_solutions[SpeedLimitSource.map] == 0.