mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-29 12:53:42 +08:00
sync with latest
This commit is contained in:
Jason Wen
@@ -219,6 +219,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
enum LongitudinalPlanSource {
|
||||
cruise @0;
|
||||
sccVision @1;
|
||||
speedLimitAssist @2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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])
|
||||
|
||||
@@ -19,3 +19,10 @@ class OffsetType(IntEnum):
|
||||
off = 0
|
||||
fixed = 1
|
||||
percentage = 2
|
||||
|
||||
|
||||
class Mode(IntEnum):
|
||||
off = 0
|
||||
information = 1
|
||||
warning = 2
|
||||
assist = 3
|
||||
|
||||
+29
-35
@@ -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
|
||||
+30
-53
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
-138
@@ -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.
|
||||
Reference in New Issue
Block a user