diff --git a/cereal/custom.capnp b/cereal/custom.capnp index ba4f4beb3f..f1894c14d1 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -144,6 +144,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { speedLimit @3 :Float32; speedLimitOffset @4 :Float32; distToSpeedLimit @5 :Float32; + source @6 :SpeedLimitSource; } enum SpeedLimitControlState { @@ -154,6 +155,12 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { adapting @4; # Reducing speed to match new speed limit. active @5; # Cruising at speed limit. } + + enum SpeedLimitSource { + none @0; + car @1; + map @2; + } } struct OnroadEventSP @0xda96579883444c35 { diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index da9f4d7771..e8a52cda08 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -41,11 +41,10 @@ def live_map_data_sp_thread_debug(gps_location_service): _sub_master.update() v_ego = _sub_master['carState'].vEgo - long_spl = _sub_master['longitudinalPlanSP'].speedLimit _policy = Policy.car_state_priority _resolver = SpeedLimitResolver(_policy) - _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master) - print(_speed_limit, _distance, _source, " <-> ", long_spl) + _resolver.update(v_ego, _sub_master) + print(_resolver.speed_limit, _resolver.distance, _resolver.source) def main(): diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index c7e16ed976..8368268977 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -52,9 +52,9 @@ class LongitudinalPlannerSP: self.events_sp.clear() # Speed Limit Control - _speed_limit, _distance, _source = self.resolver.resolve(v_ego, sm) + self.resolver.update(v_ego, sm) v_cruise_slc = self.slc.update(sm['carControl'].longActive, v_ego, a_ego, sm['carState'].vCruiseCluster, - _speed_limit, _distance, _source, self.events_sp) + self.resolver.speed_limit, self.resolver.distance, self.resolver.source, self.events_sp) v_cruise_final = min(v_cruise, v_cruise_slc) @@ -84,8 +84,9 @@ class LongitudinalPlannerSP: slc.state = self.slc.state slc.enabled = self.slc.is_enabled slc.active = self.slc.is_active - slc.speedLimit = float(self.slc.speed_limit) + slc.speedLimit = float(self.resolver.speed_limit) slc.speedLimitOffset = float(self.slc.speed_limit_offset) - slc.distToSpeedLimit = float(self.slc.distance) + slc.distToSpeedLimit = float(self.resolver.distance) + slc.source = self.resolver.source pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py index 414981a80b..7e5d117d21 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py @@ -1,12 +1,6 @@ from enum import IntEnum -class Source(IntEnum): - none = 0 - car_state = 1 - map_data = 2 - - class Policy(IntEnum): map_data_only = 0 car_state_only = 1 diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 9e24168c22..c8bb599de0 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -14,11 +14,12 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, \ SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_SPEED, CRUISE_SPEED_TOLERANCE, DISABLED_GUARD_PERIOD from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Engage, OffsetType +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Engage, OffsetType from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.selfdrive.modeld.constants import ModelConstants EventNameSP = custom.OnroadEventSP.EventName +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting) ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.pending, *ACTIVE_STATES) @@ -27,7 +28,7 @@ ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.pendi class SpeedLimitController: _speed_limit: float _distance: float - _source: Source + _source: SpeedLimitSource v_ego: float a_ego: float v_offset: float @@ -55,7 +56,7 @@ class SpeedLimitController: self.speed_limit_prev = 0. self.last_valid_speed_limit_final = 0. self._distance = 0. - self._source = Source.none + self._source = SpeedLimitSource.none self.state = SpeedLimitControlState.disabled self._state_prev = SpeedLimitControlState.disabled self.pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise @@ -90,7 +91,7 @@ class SpeedLimitController: return self._distance @property - def source(self) -> Source: + def source(self) -> SpeedLimitSource: return self._source def get_v_target_from_control(self) -> float: @@ -247,7 +248,7 @@ class SpeedLimitController: return enabled, active def update(self, long_active: bool, v_ego: float, a_ego: float, v_cruise_setpoint: float, - speed_limit: float, distance: float, source: Source, events_sp: EventsSP) -> float: + speed_limit: float, distance: float, source: SpeedLimitSource, events_sp: EventsSP) -> float: self.op_engaged = long_active self._speed_limit = speed_limit diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 64a78e4d36..941b601715 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -1,17 +1,25 @@ import time import numpy as np -from cereal import messaging +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.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource + +ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values()) class SpeedLimitResolver: - _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources - _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources + _limit_solutions: dict[SpeedLimitSource, float] # Store for speed limit solutions from different sources + _distance_solutions: dict[SpeedLimitSource, float] # Store for distance to current speed limit start for different sources _v_ego: float + speed_limit: float + distance: float + source: SpeedLimitSource def __init__(self, policy: Policy): self._gps_location_service = get_gps_location_service(Params()) @@ -20,40 +28,34 @@ class SpeedLimitResolver: self._policy = policy self._policy_to_sources_map = { - Policy.car_state_only: [Source.car_state], - Policy.car_state_priority: [Source.car_state, Source.map_data], - Policy.map_data_priority: [Source.map_data, Source.car_state], - Policy.map_data_only: [Source.map_data], - Policy.combined: [Source.car_state, Source.map_data], + 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], } - for source in Source: + for source in ALL_SOURCES: self._reset_limit_sources(source) def change_policy(self, policy: Policy) -> None: self._policy = policy - def _reset_limit_sources(self, source: Source) -> None: + def _reset_limit_sources(self, source: SpeedLimitSource) -> None: self._limit_solutions[source] = 0. self._distance_solutions[source] = 0. - def resolve(self, v_ego: float, sm: messaging.SubMaster) -> tuple[float, float, Source]: - self._v_ego = v_ego - - self._resolve_limit_sources(sm) - return self._consolidate() - 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(Source.car_state) - self._limit_solutions[Source.car_state] = sm['carStateSP'].speedLimit - self._distance_solutions[Source.car_state] = 0. + 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(Source.map_data) + self._reset_limit_sources(SpeedLimitSource.map) self._process_map_data(sm) def _process_map_data(self, sm: messaging.SubMaster) -> None: @@ -76,39 +78,44 @@ class SpeedLimitResolver: 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[Source.map_data] = speed_limit - self._distance_solutions[Source.map_data] = 0. + 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[Source.map_data] = next_speed_limit - self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead + self._limit_solutions[SpeedLimitSource.map] = next_speed_limit + self._distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead - def _consolidate(self) -> tuple[float, float, Source]: + def _consolidate(self) -> tuple[float, float, SpeedLimitSource]: source = self._get_source_solution_according_to_policy() - self.speed_limit = self._limit_solutions[source] if source else 0. - self.distance = self._distance_solutions[source] if source else 0. - self.source = source or Source.none + speed_limit = self._limit_solutions[source] if source else 0. + distance = self._distance_solutions[source] if source else 0. - return self.speed_limit, self.distance, self.source + return speed_limit, distance, source - def _get_source_solution_according_to_policy(self) -> Source | None: + def _get_source_solution_according_to_policy(self) -> 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: if self._limit_solutions[source] > 0.: - return Source(source) + return source limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float) - sources = np.array([source.value for source in sources_for_policy], dtype=int) + sources = np.array([source for source in sources_for_policy], dtype=int) if len(limits) > 0: min_idx = np.argmin(limits) - return Source(sources[min_idx]) + return sources[min_idx] - return None + return SpeedLimitSource.none + + def update(self, v_ego: float, sm: messaging.SubMaster) -> None: + self._v_ego = v_ego + self._resolve_limit_sources(sm) + + self.speed_limit, self.distance, self.source = self._consolidate() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py index f4275ef761..3f09a977b5 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py @@ -6,6 +6,8 @@ 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 from opendbc.car.toyota.values import CAR as TOYOTA from openpilot.common.constants import CV @@ -13,12 +15,14 @@ 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_controller.common import Source, OffsetType +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import OffsetType from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitControlState, REQUIRED_INITIAL_MAX_SET_SPEED, \ PRE_ACTIVE_GUARD_PERIOD from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource + ALL_STATES = tuple(SpeedLimitControlState.schema.enumerants.values()) SPEED_LIMITS = { @@ -67,7 +71,7 @@ class TestSpeedLimitController: self.slc.speed_limit_prev = 0. self.slc.last_valid_speed_limit_offsetted = 0. self.slc._distance = 0. - self.slc._source = Source.none + self.slc._source = SpeedLimitSource.none self.slc.v_cruise_setpoint = 0. self.slc.v_cruise_setpoint_prev = 0. self.events_sp.clear() @@ -86,58 +90,58 @@ class TestSpeedLimitController: def test_disabled(self): self.params.put_bool("SpeedLimitControl", False) for _ in range(int(10. / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.disabled def test_transition_disabled_to_preactive(self): for _ in range(int(3. / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.preActive assert self.slc.is_enabled and not self.slc.is_active def test_preactive_to_active_with_max_speed_confirmation(self): self.slc.state = SpeedLimitControlState.preActive - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.active assert self.slc.is_enabled and self.slc.is_active assert v_cruise_slc == SPEED_LIMITS['city'] def test_preactive_timeout_to_inactive(self): self.slc.state = SpeedLimitControlState.preActive - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)): - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.inactive def test_preactive_to_pending_no_speed_limit(self): self.slc.state = SpeedLimitControlState.preActive - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, Source.none, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.none, self.events_sp) assert self.slc.state == SpeedLimitControlState.pending assert self.slc.is_enabled and not self.slc.is_active def test_pending_to_active_when_speed_limit_available(self): self.slc.state = SpeedLimitControlState.pending - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.active def test_pending_to_adapting_when_below_speed_limit(self): self.slc.state = SpeedLimitControlState.pending - _ = self.slc.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'] + 5, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.adapting assert self.slc.is_enabled and self.slc.is_active def test_active_to_adapting_transition(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - _ = self.slc.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'] + 2, 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.adapting def test_adapting_to_active_transition(self): self.slc.state = SpeedLimitControlState.adapting self.slc.v_cruise_setpoint_prev = REQUIRED_INITIAL_MAX_SET_SPEED - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.active def test_manual_cruise_change_detection(self): @@ -146,7 +150,7 @@ class TestSpeedLimitController: self.slc.v_cruise_setpoint_prev = expected_cruise different_cruise = SPEED_LIMITS['highway'] + 5 - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.inactive @pytest.mark.parametrize("offset_type, offset_value, speed_limit, expected_offset", [ @@ -166,7 +170,7 @@ class TestSpeedLimitController: speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']] for _, speed_limit in enumerate(speed_limits): - _ = self.slc.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, speed_limit, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state in ACTIVE_STATES def test_invalid_speed_limits_handling(self): @@ -176,7 +180,7 @@ class TestSpeedLimitController: invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS] for invalid_limit in invalid_limits: - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, Source.car_state, self.events_sp) + v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, invalid_limit, 0, SpeedLimitSource.car, self.events_sp) assert isinstance(v_cruise_slc, (int, float)) assert v_cruise_slc == V_CRUISE_UNSET or v_cruise_slc > 0 @@ -185,14 +189,14 @@ class TestSpeedLimitController: old_speed_limit = SPEED_LIMITS['city'] self.slc.last_valid_speed_limit_final = old_speed_limit - v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, Source.car_state, self.events_sp) + v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, 0, 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state in ACTIVE_STATES assert v_cruise_slc == old_speed_limit def test_different_speed_limit_sources(self): self.initialize_active_state(REQUIRED_INITIAL_MAX_SET_SPEED) - for source in (Source.car_state, Source.map_data): + for source in (SpeedLimitSource.car, SpeedLimitSource.map): v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, source, self.events_sp) assert v_cruise_slc != V_CRUISE_UNSET @@ -204,14 +208,14 @@ class TestSpeedLimitController: current_speed = SPEED_LIMITS['highway'] target_speed = SPEED_LIMITS['city'] - v_cruise_slc = self.slc.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, Source.map_data, self.events_sp) + v_cruise_slc = self.slc.update(True, current_speed, 0, REQUIRED_INITIAL_MAX_SET_SPEED, target_speed, distance, SpeedLimitSource.map, self.events_sp) assert self.slc.state == SpeedLimitControlState.adapting assert v_cruise_slc == 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_slc = self.slc.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + v_cruise_slc = self.slc.update(False, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state == SpeedLimitControlState.disabled assert v_cruise_slc == V_CRUISE_UNSET @@ -232,7 +236,7 @@ class TestSpeedLimitController: initial_state = state - _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED,SPEED_LIMITS['city'], 0, SpeedLimitSource.car, self.events_sp) assert self.slc.state in ALL_STATES # Sanity check diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py index 457439022a..738c144066 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py @@ -4,11 +4,13 @@ import time import pytest from pytest_mock import MockerFixture +from cereal import custom from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE -# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy + +SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource def create_mock(properties, mocker: MockerFixture): @@ -52,22 +54,22 @@ def setup_sm_mock(mocker: MockerFixture): parametrized_policies = pytest.mark.parametrize( "policy, sm_key, function_key", [ - (Policy.car_state_only, 'carStateSP', 'car_state'), - (Policy.car_state_priority, 'carStateSP', 'car_state'), - (Policy.map_data_only, 'liveMapDataSP', 'map_data'), - (Policy.map_data_priority, 'liveMapDataSP', 'map_data'), + (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", [RefactoredSpeedLimitResolver], ids=["Refactored"]) +@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(policy) - for source in Source: + for source in ALL_SOURCES: if source in resolver._limit_solutions: assert resolver._limit_solutions[source] == 0. assert resolver._distance_solutions[source] == 0. @@ -79,22 +81,22 @@ class TestSpeedLimitResolverValidation: source_speed_limit = sm_mock[sm_key].speedLimit # Assert the resolver - speed_limit, _, source = resolver.resolve(source_speed_limit, sm_mock) - assert speed_limit == source_speed_limit - assert source == Source[function_key] + 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(Policy.combined) sm_mock = setup_sm_mock(mocker) - socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data} + 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 - speed_limit, _, source = resolver.resolve(minimum_speed_limit, sm_mock) - assert speed_limit == minimum_speed_limit - assert source == socket_to_source[minimum_key] + 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): @@ -103,9 +105,9 @@ class TestSpeedLimitResolverValidation: source_speed_limit = sm_mock[sm_key].speedLimit # Assert the parsing - speed_limit, _, source = resolver.resolve(source_speed_limit, sm_mock) - assert resolver._limit_solutions[Source[function_key]] == source_speed_limit - assert resolver._distance_solutions[Source[function_key]] == 0. + 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): @@ -113,12 +115,12 @@ class TestSpeedLimitResolverValidation: resolver = resolver_class(policy) sm_mock = setup_sm_mock(mocker) - _speed_limit, _distance, _source = resolver.resolve(v_ego, sm_mock) + resolver.update(v_ego, sm_mock) # After resolution - assert _speed_limit is not None - assert _distance is not None - assert _source is not None + 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): @@ -126,5 +128,5 @@ class TestSpeedLimitResolverValidation: 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[Source.map_data] == 0. - assert resolver._distance_solutions[Source.map_data] == 0. + assert resolver._limit_solutions[SpeedLimitSource.map] == 0. + assert resolver._distance_solutions[SpeedLimitSource.map] == 0.