mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-08 18:13:45 +08:00
refactor speed limit resolver
This commit is contained in:
Jason Wen
@@ -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 {
|
||||
|
||||
@@ -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():
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
+24
-20
@@ -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
|
||||
|
||||
|
||||
+27
-25
@@ -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.
|
||||
|
||||
Reference in New Issue
Block a user