mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-11-03 14:53:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

more resolver refactor

Browse Source
This commit is contained in:
Jason Wen
2025-09-09 02:04:24 -04:00
parent 51d2d7d5f5
commit 29ff34821c
4 changed files with 34 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/mapd/live_map_data/debug.py
+2 -2
View File
@@ -41,8 +41,8 @@ def live_map_data_sp_thread_debug(gps_location_service):
_sub_master.update()
v_ego = _sub_master['carState'].vEgo
_policy = Policy.car_state_priority
_resolver = SpeedLimitResolver(_policy)
_resolver = SpeedLimitResolver()
_resolver.policy = Policy.car_state_priority
_resolver.update(v_ego, _sub_master)
print(_resolver.speed_limit, _resolver.distance, _resolver.source)
sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+1 -12
View File
@@ -8,9 +8,7 @@ See the LICENSE.md file in the root directory for more details.
from cereal import messaging, custom
from opendbc.car import structs
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
@@ -23,10 +21,8 @@ class LongitudinalPlannerSP:
def __init__(self, CP: structs.CarParams, mpc):
self.events_sp = EventsSP()
self.params = Params()
self.frame = -1
self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True)
self.resolver = SpeedLimitResolver(self.policy)
self.resolver = SpeedLimitResolver()
self.dec = DynamicExperimentalController(CP, mpc)
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
@@ -37,11 +33,6 @@ class LongitudinalPlannerSP:
# If we don't have a generation set, we assume it's default model. Which as of today are mlsim.
return bool(self.generation is None or self.generation >= 11)
def update_params(self):
if self.frame % int(3. / DT_MDL) == 0:
self.policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True))
self.resolver.change_policy(self.policy)
def get_mpc_mode(self) -> str | None:
if not self.dec.active():
return None
@@ -63,8 +54,6 @@ class LongitudinalPlannerSP:
def update(self, sm: messaging.SubMaster) -> None:
self.dec.update(sm)
self.frame += 1
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
plan_sp_send = messaging.new_message('longitudinalPlanSP')
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
+19 -7
View File
@@ -5,7 +5,8 @@ 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.common.realtime import DT_MDL
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource
@@ -21,12 +22,15 @@ class SpeedLimitResolver:
distance: float
source: SpeedLimitSource
def __init__(self, policy: Policy):
self._gps_location_service = get_gps_location_service(Params())
def __init__(self):
self.params = Params()
self.frame = -1
self._gps_location_service = get_gps_location_service(self.params)
self._limit_solutions = {}
self._distance_solutions = {}
self._policy = policy
self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True)
self._policy_to_sources_map = {
Policy.car_state_only: [SpeedLimitSource.car],
Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
@@ -37,8 +41,13 @@ class SpeedLimitResolver:
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("SpeedLimitControlPolicy", return_default=True))
self.change_policy(self.policy)
def change_policy(self, policy: Policy) -> None:
self._policy = policy
self.policy = policy
def _reset_limit_sources(self, source: SpeedLimitSource) -> None:
self._limit_solutions[source] = 0.
@@ -97,9 +106,9 @@ class SpeedLimitResolver:
return speed_limit, distance, source
def _get_source_solution_according_to_policy(self) -> SpeedLimitSource:
sources_for_policy = self._policy_to_sources_map[self._policy]
sources_for_policy = self._policy_to_sources_map[self.policy]
if self._policy != Policy.combined:
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.:
@@ -116,6 +125,9 @@ class SpeedLimitResolver:
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
sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py
+12 -6
View File
@@ -68,7 +68,8 @@ 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)
resolver = resolver_class()
resolver.policy = policy
for source in ALL_SOURCES:
if source in resolver._limit_solutions:
assert resolver._limit_solutions[source] == 0.
@@ -76,7 +77,8 @@ class TestSpeedLimitResolverValidation:
@parametrized_policies
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
resolver = resolver_class(policy)
resolver = resolver_class()
resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
@@ -86,7 +88,8 @@ class TestSpeedLimitResolverValidation:
assert resolver.source == ALL_SOURCES[function_key]
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
resolver = resolver_class(Policy.combined)
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(
@@ -100,7 +103,8 @@ class TestSpeedLimitResolverValidation:
@parametrized_policies
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
resolver = resolver_class(policy)
resolver = resolver_class()
resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
source_speed_limit = sm_mock[sm_key].speedLimit
@@ -112,7 +116,8 @@ class TestSpeedLimitResolverValidation:
@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(policy)
resolver = resolver_class()
resolver.policy = policy
sm_mock = setup_sm_mock(mocker)
resolver.update(v_ego, sm_mock)
@@ -124,7 +129,8 @@ class TestSpeedLimitResolverValidation:
@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(policy)
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)