mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-25 16:12:08 +08:00
Reimplement SLC
This commit is contained in:
DevTekVE
@@ -209,10 +209,14 @@ selfdrive/controls/lib/lateral_planner.py
|
||||
selfdrive/controls/lib/longcontrol.py
|
||||
selfdrive/controls/lib/longitudinal_planner.py
|
||||
selfdrive/controls/lib/pid.py
|
||||
selfdrive/controls/lib/speed_limit_controller.py
|
||||
selfdrive/controls/lib/turn_speed_controller.py
|
||||
selfdrive/controls/lib/vehicle_model.py
|
||||
selfdrive/controls/lib/vision_turn_controller.py
|
||||
selfdrive/controls/lib/sunnypilot/__init__.py
|
||||
selfdrive/controls/lib/sunnypilot/common.py
|
||||
selfdrive/controls/lib/sunnypilot/helpers.py
|
||||
selfdrive/controls/lib/sunnypilot/speed_limit_controller.py
|
||||
selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py
|
||||
|
||||
selfdrive/controls/lib/lateral_mpc_lib/.gitignore
|
||||
selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
|
||||
|
||||
@@ -9,14 +9,17 @@ import cereal.messaging as messaging
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import SpeedLimitController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import SpeedLimitController
|
||||
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
|
||||
from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController
|
||||
from openpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver
|
||||
from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController
|
||||
from openpilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController
|
||||
from openpilot.selfdrive.controls.lib.events import Events
|
||||
@@ -225,7 +228,7 @@ class LongitudinalPlanner:
|
||||
longitudinalPlanSP.speedLimit = float(self.speed_limit_controller.speed_limit)
|
||||
longitudinalPlanSP.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset)
|
||||
longitudinalPlanSP.distToSpeedLimit = float(self.speed_limit_controller.distance)
|
||||
longitudinalPlanSP.isMapSpeedLimit = bool(self.speed_limit_controller.source not in (SpeedLimitResolver.Source.none, SpeedLimitResolver.Source.nav))
|
||||
longitudinalPlanSP.isMapSpeedLimit = bool(self.speed_limit_controller.source not in (Source.none, Source.nav))
|
||||
longitudinalPlanSP.events = self.events.to_msg()
|
||||
|
||||
longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
|
||||
|
||||
@@ -1,428 +0,0 @@
|
||||
import numpy as np
|
||||
import time
|
||||
from common.numpy_fast import interp
|
||||
from enum import IntEnum
|
||||
from cereal import custom, car
|
||||
from common.conversions import Conversions as CV
|
||||
from common.params import Params
|
||||
from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \
|
||||
LIMIT_MAX_MAP_DATA_AGE, CONTROL_N
|
||||
from selfdrive.controls.lib.events import Events, ET
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
|
||||
_PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates.
|
||||
_TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal.
|
||||
|
||||
# Lookup table for speed limit percent offset depending on speed.
|
||||
_LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h
|
||||
_LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h
|
||||
|
||||
SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
_DEBUG = False
|
||||
|
||||
|
||||
def _debug(msg):
|
||||
if not _DEBUG:
|
||||
return
|
||||
print(msg)
|
||||
|
||||
|
||||
def _description_for_state(speed_limit_control_state):
|
||||
if speed_limit_control_state == SpeedLimitControlState.inactive:
|
||||
return 'INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.tempInactive:
|
||||
return 'TEMP_INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.adapting:
|
||||
return 'ADAPTING'
|
||||
if speed_limit_control_state == SpeedLimitControlState.active:
|
||||
return 'ACTIVE'
|
||||
|
||||
|
||||
class SpeedLimitResolver():
|
||||
class Source(IntEnum):
|
||||
none = 0
|
||||
car_state = 1
|
||||
map_data = 2
|
||||
nav = 3
|
||||
|
||||
class Policy(IntEnum):
|
||||
car_state_only = 0
|
||||
map_data_only = 1
|
||||
car_state_priority = 2
|
||||
map_data_priority = 3
|
||||
combined = 4
|
||||
nav_only = 5
|
||||
nav_priority = 6
|
||||
|
||||
def __init__(self, policy=Policy.nav_priority):
|
||||
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._v_ego = 0.
|
||||
self._current_speed_limit = 0.
|
||||
self._policy = policy
|
||||
self._next_speed_limit_prev = 0.
|
||||
self.speed_limit = 0.
|
||||
self.distance = 0.
|
||||
self.source = SpeedLimitResolver.Source.none
|
||||
|
||||
def resolve(self, v_ego, current_speed_limit, sm):
|
||||
self._v_ego = v_ego
|
||||
self._current_speed_limit = current_speed_limit
|
||||
self._sm = sm
|
||||
|
||||
self._get_from_car_state()
|
||||
self._get_from_nav()
|
||||
self._get_from_map_data()
|
||||
self._consolidate()
|
||||
|
||||
return self.speed_limit, self.distance, self.source
|
||||
|
||||
def _get_from_car_state(self):
|
||||
self._limit_solutions[SpeedLimitResolver.Source.car_state] = self._sm['carState'].cruiseState.speedLimit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.car_state] = 0.
|
||||
|
||||
def _get_from_nav(self):
|
||||
# Ignore if nav instruction is not alive
|
||||
if not self._sm.alive['navInstruction']:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.nav] = 0.
|
||||
self._distance_solutions[SpeedLimitResolver.Source.nav] = 0.
|
||||
_debug('SL: No nav instruction for speed limit')
|
||||
return
|
||||
|
||||
# Load limits from nav instruction
|
||||
self._limit_solutions[SpeedLimitResolver.Source.nav] = self._sm['navInstruction'].speedLimit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.nav] = 0.
|
||||
|
||||
def _get_from_map_data(self):
|
||||
# Ignore if no live map data
|
||||
sock = 'liveMapDataSP'
|
||||
if self._sm.logMonoTime[sock] is None:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
_debug('SL: No map data for speed limit')
|
||||
return
|
||||
|
||||
# Load limits from map_data
|
||||
map_data = self._sm[sock]
|
||||
speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
|
||||
next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
|
||||
|
||||
# Calculate the age of the gps fix. Ignore if too old.
|
||||
gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
|
||||
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
_debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
|
||||
return
|
||||
|
||||
# When we have no ahead speed limit to consider or it is greater than current speed limit
|
||||
# or car has stopped, then provide current value and reset tracking.
|
||||
if next_speed_limit == 0. or self._v_ego <= 0. or next_speed_limit > self._current_speed_limit:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = speed_limit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
self._next_speed_limit_prev = 0.
|
||||
return
|
||||
|
||||
# Calculate the actual distance to the speed limit ahead corrected by gps_fix_age
|
||||
distance_since_fix = self._v_ego * gps_fix_age
|
||||
distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
|
||||
|
||||
# When we have a next_speed_limit value that has not changed from a provided next speed limit value
|
||||
# in previous resolutions, we keep providing it.
|
||||
if next_speed_limit == self._next_speed_limit_prev:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = next_speed_limit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = distance_to_speed_limit_ahead
|
||||
return
|
||||
|
||||
# Reset tracking
|
||||
self._next_speed_limit_prev = 0.
|
||||
|
||||
# Calculated the time needed to adapt to the new limit and the corresponding distance.
|
||||
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
|
||||
|
||||
# When we detect we are close enough, we provide the next limit value and track it.
|
||||
if distance_to_speed_limit_ahead <= adapt_distance:
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = next_speed_limit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = distance_to_speed_limit_ahead
|
||||
self._next_speed_limit_prev = next_speed_limit
|
||||
return
|
||||
|
||||
# Otherwise we just provide the map data speed limit.
|
||||
self.distance_to_map_speed_limit = 0.
|
||||
self._limit_solutions[SpeedLimitResolver.Source.map_data] = speed_limit
|
||||
self._distance_solutions[SpeedLimitResolver.Source.map_data] = 0.
|
||||
|
||||
def _consolidate(self):
|
||||
limits = np.array([], dtype=float)
|
||||
distances = np.array([], dtype=float)
|
||||
sources = np.array([], dtype=int)
|
||||
|
||||
if self._policy == SpeedLimitResolver.Policy.car_state_only or \
|
||||
self._policy == SpeedLimitResolver.Policy.car_state_priority or \
|
||||
self._policy == SpeedLimitResolver.Policy.combined:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.car_state])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.car_state])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.car_state.value)
|
||||
|
||||
if self._policy == SpeedLimitResolver.Policy.nav_only or \
|
||||
self._policy == SpeedLimitResolver.Policy.nav_priority or \
|
||||
self._policy == SpeedLimitResolver.Policy.combined:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.nav])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.nav])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.nav.value)
|
||||
|
||||
if self._policy == SpeedLimitResolver.Policy.map_data_only or \
|
||||
self._policy == SpeedLimitResolver.Policy.map_data_priority or \
|
||||
self._policy == SpeedLimitResolver.Policy.combined:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.map_data])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.map_data])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.map_data.value)
|
||||
|
||||
if np.amax(limits) == 0.:
|
||||
if self._policy == SpeedLimitResolver.Policy.car_state_priority:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.map_data])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.map_data])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.map_data.value)
|
||||
|
||||
elif self._policy == SpeedLimitResolver.Policy.map_data_priority:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.car_state])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.car_state])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.car_state.value)
|
||||
|
||||
elif self._policy == SpeedLimitResolver.Policy.nav_priority:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.map_data])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.map_data])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.map_data.value)
|
||||
|
||||
if np.amax(limits) == 0.:
|
||||
limits = np.append(limits, self._limit_solutions[SpeedLimitResolver.Source.car_state])
|
||||
distances = np.append(distances, self._distance_solutions[SpeedLimitResolver.Source.car_state])
|
||||
sources = np.append(sources, SpeedLimitResolver.Source.car_state.value)
|
||||
|
||||
# Get all non-zero values and set the minimum if any, otherwise 0.
|
||||
mask = limits > 0.
|
||||
limits = limits[mask]
|
||||
distances = distances[mask]
|
||||
sources = sources[mask]
|
||||
|
||||
if len(limits) > 0:
|
||||
min_idx = np.argmin(limits)
|
||||
self.speed_limit = limits[min_idx]
|
||||
self.distance = distances[min_idx]
|
||||
self.source = SpeedLimitResolver.Source(sources[min_idx])
|
||||
else:
|
||||
self.speed_limit = 0.
|
||||
self.distance = 0.
|
||||
self.source = SpeedLimitResolver.Source.none
|
||||
|
||||
_debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
|
||||
|
||||
|
||||
class SpeedLimitController():
|
||||
def __init__(self):
|
||||
self._params = Params()
|
||||
self._resolver = SpeedLimitResolver()
|
||||
self._last_params_update = 0.0
|
||||
self._last_op_enabled_time = 0.0
|
||||
self._is_metric = self._params.get_bool("IsMetric")
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset")
|
||||
self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator")
|
||||
self._op_enabled = False
|
||||
self._op_enabled_prev = False
|
||||
self._v_ego = 0.
|
||||
self._a_ego = 0.
|
||||
self._v_offset = 0.
|
||||
self._v_cruise_setpoint = 0.
|
||||
self._v_cruise_setpoint_prev = 0.
|
||||
self._v_cruise_setpoint_changed = False
|
||||
self._speed_limit = 0.
|
||||
self._speed_limit_prev = 0.
|
||||
self._speed_limit_changed = False
|
||||
self._distance = 0.
|
||||
self._source = SpeedLimitResolver.Source.none
|
||||
self._state = SpeedLimitControlState.inactive
|
||||
self._state_prev = SpeedLimitControlState.inactive
|
||||
self._gas_pressed = False
|
||||
self._a_target = 0.
|
||||
|
||||
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
|
||||
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
|
||||
self._brake_pressed = False
|
||||
self._brake_pressed_prev = False
|
||||
|
||||
@property
|
||||
def a_target(self):
|
||||
return self._a_target if self.is_active else self._a_ego
|
||||
|
||||
@property
|
||||
def state(self):
|
||||
return self._state
|
||||
|
||||
@state.setter
|
||||
def state(self, value):
|
||||
if value != self._state:
|
||||
_debug(f'Speed Limit Controller state: {_description_for_state(value)}')
|
||||
|
||||
if value == SpeedLimitControlState.tempInactive:
|
||||
# Reset previous speed limit to current value as to prevent going out of tempInactive in
|
||||
# a single cycle when the speed limit changes at the same time the user has temporarily deactivate it.
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
|
||||
self._state = value
|
||||
|
||||
@property
|
||||
def is_active(self):
|
||||
return self.state > SpeedLimitControlState.tempInactive
|
||||
|
||||
@property
|
||||
def speed_limit_offseted(self):
|
||||
return self._speed_limit + self.speed_limit_offset
|
||||
|
||||
@property
|
||||
def speed_limit_offset(self):
|
||||
if self._offset_enabled:
|
||||
if self._offset_type == 0:
|
||||
return interp(self._speed_limit, _LIMIT_PERC_OFFSET_BP, _LIMIT_PERC_OFFSET_V) * self._speed_limit
|
||||
elif self._offset_type == 1:
|
||||
return self._offset_value * 0.01 * self._speed_limit
|
||||
elif self._offset_type == 2:
|
||||
return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
|
||||
return 0.
|
||||
|
||||
@property
|
||||
def speed_limit(self):
|
||||
return self._speed_limit
|
||||
|
||||
@property
|
||||
def distance(self):
|
||||
return self._distance
|
||||
|
||||
@property
|
||||
def source(self):
|
||||
return self._source
|
||||
|
||||
def _update_params(self):
|
||||
t = time.monotonic()
|
||||
if t > self._last_params_update + _PARAMS_UPDATE_PERIOD:
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._offset_enabled = self._params.get_bool("SpeedLimitPercOffset")
|
||||
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
|
||||
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
|
||||
_debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_enabled}')
|
||||
self._last_params_update = t
|
||||
|
||||
def _update_calculations(self):
|
||||
# Update current velocity offset (error)
|
||||
self._v_offset = self.speed_limit_offseted - self._v_ego
|
||||
|
||||
# Track the time op becomes active to prevent going to tempInactive right away after
|
||||
# op enabling since controlsd will change the cruise speed every time on enabling and this will
|
||||
# cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
|
||||
# speed.
|
||||
if not self._op_enabled_prev and self._op_enabled:
|
||||
self._last_op_enabled_time = time.monotonic()
|
||||
|
||||
# Update change tracking variables
|
||||
self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
|
||||
self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
self._v_cruise_setpoint_prev = self._v_cruise_setpoint
|
||||
self._op_enabled_prev = self._op_enabled
|
||||
self._brake_pressed_prev = self._brake_pressed
|
||||
|
||||
def _state_transition(self):
|
||||
self._state_prev = self._state
|
||||
|
||||
# In any case, if op is disabled, or speed limit control is disabled
|
||||
# or the reported speed limit is 0 or gas is pressed, deactivate.
|
||||
if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or (self._gas_pressed and self._disengage_on_accelerator):
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
return
|
||||
|
||||
# In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
|
||||
# Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations
|
||||
# due to controlsd logic changing cruise setpoint when going active.
|
||||
if self._v_cruise_setpoint_changed and \
|
||||
time.monotonic() > (self._last_op_enabled_time + _TEMP_INACTIVE_GUARD_PERIOD):
|
||||
self.state = SpeedLimitControlState.tempInactive
|
||||
return
|
||||
|
||||
# inactive
|
||||
if self.state == SpeedLimitControlState.inactive:
|
||||
# If the limit speed offset is negative (i.e. reduce speed) and lower than threshold
|
||||
# we go to adapting state to quickly reduce speed, otherwise we go directly to active
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
else:
|
||||
self.state = SpeedLimitControlState.active
|
||||
# tempInactive
|
||||
elif self.state == SpeedLimitControlState.tempInactive:
|
||||
# if speed limit changes, transition to inactive,
|
||||
# proper active state will be set on next iteration.
|
||||
if self._speed_limit_changed:
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
# adapting
|
||||
elif self.state == SpeedLimitControlState.adapting:
|
||||
# Go to active once the speed offset is over threshold.
|
||||
if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.active
|
||||
# active
|
||||
elif self.state == SpeedLimitControlState.active:
|
||||
# Go to adapting if the speed offset goes below threshold.
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
|
||||
def _update_solution(self):
|
||||
# inactive or tempInactive state
|
||||
if self.state <= SpeedLimitControlState.tempInactive:
|
||||
# Preserve current values
|
||||
a_target = self._a_ego
|
||||
# adapting
|
||||
elif self.state == SpeedLimitControlState.adapting:
|
||||
# When adapting we target to achieve the speed limit on the distance if not there yet,
|
||||
# otherwise try to keep the speed constant around the control time horizon.
|
||||
if self.distance > 0:
|
||||
a_target = (self.speed_limit_offseted**2 - self._v_ego**2) / (2. * self.distance)
|
||||
else:
|
||||
a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
|
||||
# active
|
||||
elif self.state == SpeedLimitControlState.active:
|
||||
# When active we are trying to keep the speed constant around the control time horizon.
|
||||
a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
|
||||
|
||||
# Keep solution limited.
|
||||
self._a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
|
||||
|
||||
def _update_events(self, events):
|
||||
if not self.is_active:
|
||||
# no event while inactive
|
||||
return
|
||||
|
||||
if self._state_prev <= SpeedLimitControlState.tempInactive:
|
||||
events.add(EventName.speedLimitActive)
|
||||
elif self._speed_limit_changed != 0:
|
||||
events.add(EventName.speedLimitValueChange)
|
||||
|
||||
def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, events=Events()):
|
||||
_car_state = sm['carState']
|
||||
self._op_enabled = enabled and sm['controlsState'].enabled and _car_state.cruiseState.enabled and \
|
||||
not (_car_state.brakePressed and (not self._brake_pressed_prev or not _car_state.standstill)) and \
|
||||
not events.contains(ET.OVERRIDE_LONGITUDINAL)
|
||||
self._v_ego = v_ego
|
||||
self._a_ego = a_ego
|
||||
self._v_cruise_setpoint = v_cruise_setpoint
|
||||
self._gas_pressed = _car_state.gasPressed
|
||||
self._brake_pressed = _car_state.brakePressed
|
||||
|
||||
self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)
|
||||
|
||||
self._update_params()
|
||||
self._update_calculations()
|
||||
self._state_transition()
|
||||
self._update_solution()
|
||||
self._update_events(events)
|
||||
@@ -0,0 +1,13 @@
|
||||
from cereal import custom, car
|
||||
|
||||
DEBUG = False
|
||||
PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates.
|
||||
TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal.
|
||||
|
||||
# Lookup table for speed limit percent offset depending on speed.
|
||||
LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h
|
||||
LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h
|
||||
|
||||
SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
|
||||
EventName = car.CarEvent.EventName
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
from enum import IntEnum
|
||||
|
||||
|
||||
class Source(IntEnum):
|
||||
none = 0
|
||||
car_state = 1
|
||||
map_data = 2
|
||||
nav = 3
|
||||
|
||||
|
||||
class Policy(IntEnum):
|
||||
car_state_only = 0
|
||||
map_data_only = 1
|
||||
car_state_priority = 2
|
||||
map_data_priority = 3
|
||||
combined = 4
|
||||
nav_only = 5
|
||||
nav_priority = 6
|
||||
@@ -0,0 +1,19 @@
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot import DEBUG, SpeedLimitControlState
|
||||
from openpilot.system.swaglog import cloudlog
|
||||
|
||||
|
||||
def debug(msg):
|
||||
if not DEBUG:
|
||||
return
|
||||
cloudlog.debug(msg)
|
||||
|
||||
|
||||
def description_for_state(speed_limit_control_state):
|
||||
if speed_limit_control_state == SpeedLimitControlState.inactive:
|
||||
return 'INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.tempInactive:
|
||||
return 'TEMP_INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.adapting:
|
||||
return 'ADAPTING'
|
||||
if speed_limit_control_state == SpeedLimitControlState.active:
|
||||
return 'ACTIVE'
|
||||
@@ -0,0 +1,235 @@
|
||||
import numpy as np
|
||||
import time
|
||||
from common.numpy_fast import interp
|
||||
from common.conversions import Conversions as CV
|
||||
from common.params import Params
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
|
||||
PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, EventName, SpeedLimitControlState
|
||||
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \
|
||||
CONTROL_N
|
||||
from openpilot.selfdrive.controls.lib.events import Events, ET
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source, Policy
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.helpers import description_for_state, debug
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
|
||||
class SpeedLimitController:
|
||||
def __init__(self):
|
||||
self._params = Params()
|
||||
self._resolver = SpeedLimitResolver()
|
||||
self._last_params_update = 0.0
|
||||
self._last_op_enabled_time = 0.0
|
||||
self._is_metric = self._params.get_bool("IsMetric")
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator")
|
||||
self._op_enabled = False
|
||||
self._op_enabled_prev = False
|
||||
self._v_ego = 0.
|
||||
self._a_ego = 0.
|
||||
self._v_offset = 0.
|
||||
self._v_cruise_setpoint = 0.
|
||||
self._v_cruise_setpoint_prev = 0.
|
||||
self._v_cruise_setpoint_changed = False
|
||||
self._speed_limit = 0.
|
||||
self._speed_limit_prev = 0.
|
||||
self._speed_limit_changed = False
|
||||
self._distance = 0.
|
||||
self._source = Source.none
|
||||
self._state = SpeedLimitControlState.inactive
|
||||
self._state_prev = SpeedLimitControlState.inactive
|
||||
self._gas_pressed = False
|
||||
self._a_target = 0.
|
||||
|
||||
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
|
||||
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
|
||||
self._brake_pressed = False
|
||||
self._brake_pressed_prev = False
|
||||
|
||||
# Mapping functions to state transitions
|
||||
self.state_transition_strategy = {
|
||||
# Transition functions for each state
|
||||
SpeedLimitControlState.inactive: self.transition_state_from_inactive,
|
||||
SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive,
|
||||
SpeedLimitControlState.adapting: self.transition_state_from_adapting,
|
||||
SpeedLimitControlState.active: self.transition_state_from_active,
|
||||
}
|
||||
|
||||
# Solution functions mapped to respective states
|
||||
self.acceleration_solutions = {
|
||||
# Solution functions for each state
|
||||
SpeedLimitControlState.tempInactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitControlState.inactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration,
|
||||
SpeedLimitControlState.active: self.get_active_state_target_acceleration,
|
||||
}
|
||||
|
||||
@property
|
||||
def a_target(self):
|
||||
return self._a_target if self.is_active else self._a_ego
|
||||
|
||||
@property
|
||||
def state(self):
|
||||
return self._state
|
||||
|
||||
@state.setter
|
||||
def state(self, value):
|
||||
if value != self._state:
|
||||
debug(f'Speed Limit Controller state: {description_for_state(value)}')
|
||||
|
||||
if value == SpeedLimitControlState.tempInactive:
|
||||
# Reset previous speed limit to current value as to prevent going out of tempInactive in
|
||||
# a single cycle when the speed limit changes at the same time the user has temporarily deactivated it.
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
|
||||
self._state = value
|
||||
|
||||
@property
|
||||
def is_active(self):
|
||||
return self.state > SpeedLimitControlState.tempInactive
|
||||
|
||||
@property
|
||||
def speed_limit_offseted(self):
|
||||
return self._speed_limit + self.speed_limit_offset
|
||||
|
||||
@property
|
||||
def speed_limit_offset(self):
|
||||
if self._offset_type == 0:
|
||||
return interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit
|
||||
elif self._offset_type == 1:
|
||||
return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
|
||||
elif self._offset_type == 2:
|
||||
return self._offset_value * 0.01 * self._speed_limit
|
||||
return 0.
|
||||
|
||||
@property
|
||||
def speed_limit(self):
|
||||
return self._speed_limit
|
||||
|
||||
@property
|
||||
def distance(self):
|
||||
return self._distance
|
||||
|
||||
@property
|
||||
def source(self):
|
||||
return self._source
|
||||
|
||||
def _update_params(self):
|
||||
t = time.monotonic()
|
||||
if t > self._last_params_update + PARAMS_UPDATE_PERIOD:
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
|
||||
self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
|
||||
debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}')
|
||||
self._last_params_update = t
|
||||
|
||||
def _update_calculations(self):
|
||||
# Update current velocity offset (error)
|
||||
self._v_offset = self.speed_limit_offseted - self._v_ego
|
||||
|
||||
# Track the time op becomes active to prevent going to tempInactive right away after
|
||||
# op enabling since controlsd will change the cruise speed every time on enabling and this will
|
||||
# cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
|
||||
# speed.
|
||||
if not self._op_enabled_prev and self._op_enabled:
|
||||
self._last_op_enabled_time = time.monotonic()
|
||||
|
||||
# Update change tracking variables
|
||||
self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
|
||||
self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
self._v_cruise_setpoint_prev = self._v_cruise_setpoint
|
||||
self._op_enabled_prev = self._op_enabled
|
||||
self._brake_pressed_prev = self._brake_pressed
|
||||
|
||||
def transition_state_from_inactive(self):
|
||||
""" Make state transition from inactive state """
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
else:
|
||||
self.state = SpeedLimitControlState.active
|
||||
|
||||
def transition_state_from_temp_inactive(self):
|
||||
""" Make state transition from temporary inactive state """
|
||||
if self._speed_limit_changed:
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
|
||||
def transition_state_from_adapting(self):
|
||||
""" Make state transition from adapting state """
|
||||
if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.active
|
||||
|
||||
def transition_state_from_active(self):
|
||||
""" Make state transition from active state """
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
|
||||
def _state_transition(self):
|
||||
self._state_prev = self._state
|
||||
|
||||
# In any case, if op is disabled, or speed limit control is disabled
|
||||
# or the reported speed limit is 0 or gas is pressed, deactivate.
|
||||
if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or (self._gas_pressed and self._disengage_on_accelerator):
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
return
|
||||
|
||||
# In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
|
||||
# Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations
|
||||
# due to controlsd logic changing cruise setpoint when going active.
|
||||
if self._v_cruise_setpoint_changed and \
|
||||
time.monotonic() > (self._last_op_enabled_time + TEMP_INACTIVE_GUARD_PERIOD):
|
||||
self.state = SpeedLimitControlState.tempInactive
|
||||
return
|
||||
|
||||
self.state_transition_strategy[self.state]()
|
||||
|
||||
def get_current_acceleration_as_target(self):
|
||||
""" When state is inactive or tempInactive, preserve current acceleration """
|
||||
return self._a_ego
|
||||
|
||||
def get_adapting_state_target_acceleration(self):
|
||||
""" In adapting state, calculate target acceleration based on speed limit and current velocity """
|
||||
if self.distance > 0:
|
||||
return (self.speed_limit_offseted**2 - self._v_ego**2) / (2. * self.distance)
|
||||
|
||||
return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
|
||||
|
||||
def get_active_state_target_acceleration(self):
|
||||
""" In active state, aim to keep speed constant around control time horizon """
|
||||
return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
|
||||
|
||||
def _update_solution(self):
|
||||
a_target = self.acceleration_solutions[self.state]()
|
||||
|
||||
# Keep solution limited.
|
||||
self._a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
|
||||
|
||||
def _update_events(self, events):
|
||||
if not self.is_active:
|
||||
# no event while inactive
|
||||
return
|
||||
|
||||
if self._state_prev <= SpeedLimitControlState.tempInactive:
|
||||
events.add(EventName.speedLimitActive)
|
||||
elif self._speed_limit_changed != 0:
|
||||
events.add(EventName.speedLimitValueChange)
|
||||
|
||||
def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, events=Events()):
|
||||
_car_state = sm['carState']
|
||||
self._op_enabled = enabled and sm['controlsState'].enabled and _car_state.cruiseState.enabled and \
|
||||
not (_car_state.brakePressed and (not self._brake_pressed_prev or not _car_state.standstill)) and \
|
||||
not events.contains(ET.OVERRIDE_LONGITUDINAL)
|
||||
self._v_ego = v_ego
|
||||
self._a_ego = a_ego
|
||||
self._v_cruise_setpoint = v_cruise_setpoint
|
||||
self._gas_pressed = _car_state.gasPressed
|
||||
self._brake_pressed = _car_state.brakePressed
|
||||
|
||||
self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)
|
||||
|
||||
self._update_params()
|
||||
self._update_calculations()
|
||||
self._state_transition()
|
||||
self._update_solution()
|
||||
self._update_events(events)
|
||||
@@ -0,0 +1,122 @@
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source, Policy
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.helpers import debug
|
||||
|
||||
|
||||
class SpeedLimitResolver:
|
||||
|
||||
def __init__(self, policy=Policy.nav_priority):
|
||||
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 = policy
|
||||
self._policy_to_sources_map = {
|
||||
Policy.car_state_only: [Source.car_state],
|
||||
Policy.car_state_priority: [Source.car_state, Source.nav, Source.map_data],
|
||||
Policy.map_data_priority: [Source.map_data, Source.nav, Source.car_state],
|
||||
Policy.nav_priority: [Source.nav, Source.map_data, Source.car_state],
|
||||
Policy.map_data_only: [Source.map_data],
|
||||
Policy.combined: [Source.car_state, Source.nav, Source.map_data],
|
||||
Policy.nav_only: [Source.nav]
|
||||
}
|
||||
self._reset_limit_sources()
|
||||
|
||||
def _reset_limit_sources(self):
|
||||
self._limit_solutions.clear()
|
||||
self._distance_solutions.clear()
|
||||
for source in Source:
|
||||
self._limit_solutions[source] = 0.
|
||||
self._distance_solutions[source] = 0.
|
||||
|
||||
def resolve(self, v_ego, current_speed_limit, sm):
|
||||
self._reset_limit_sources()
|
||||
self._v_ego = v_ego
|
||||
self._current_speed_limit = current_speed_limit
|
||||
self._sm = sm
|
||||
|
||||
self._resolve_limit_sources()
|
||||
return self._consolidate()
|
||||
|
||||
def _resolve_limit_sources(self):
|
||||
"""Get limit solutions from each data source"""
|
||||
self._get_from_car_state()
|
||||
self._get_from_nav()
|
||||
self._get_from_map_data()
|
||||
|
||||
def _get_from_car_state(self):
|
||||
self._limit_solutions[Source.car_state] = self._sm['carState'].cruiseState.speedLimit
|
||||
self._distance_solutions[Source.car_state] = 0.
|
||||
|
||||
def _get_from_nav(self):
|
||||
if not self._sm.alive['navInstruction']:
|
||||
debug('SL: No nav instruction for speed limit')
|
||||
return
|
||||
|
||||
# Load limits from nav instruction
|
||||
self._limit_solutions[Source.nav] = self._sm['navInstruction'].speedLimit
|
||||
self._distance_solutions[Source.nav] = 0.
|
||||
|
||||
def _get_from_map_data(self):
|
||||
sock = 'liveMapDataSP'
|
||||
if self._sm.logMonoTime[sock] is None:
|
||||
debug('SL: No map data for speed limit')
|
||||
return
|
||||
|
||||
# Load limits from map_data
|
||||
self._process_map_data(self._sm[sock])
|
||||
|
||||
def _process_map_data(self, map_data):
|
||||
speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
|
||||
next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
|
||||
|
||||
gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
|
||||
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
|
||||
debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
|
||||
return
|
||||
|
||||
self._calculate_map_data_limits(speed_limit, next_speed_limit, map_data)
|
||||
|
||||
def _calculate_map_data_limits(self, speed_limit, next_speed_limit, map_data):
|
||||
distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 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.
|
||||
|
||||
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
|
||||
|
||||
def _consolidate(self):
|
||||
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
|
||||
|
||||
debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
|
||||
return self.speed_limit, self.distance, self.source
|
||||
|
||||
def _get_source_solution_according_to_policy(self) -> Source | None:
|
||||
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
|
||||
|
||||
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)
|
||||
|
||||
if len(limits) > 0:
|
||||
min_idx = np.argmin(limits)
|
||||
return sources[min_idx]
|
||||
|
||||
return None
|
||||
@@ -0,0 +1,132 @@
|
||||
import random
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from unittest.mock import MagicMock
|
||||
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_MAX_MAP_DATA_AGE
|
||||
|
||||
# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver
|
||||
from selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Source
|
||||
from openpilot.selfdrive.controls.lib.sunnypilot.common import Policy
|
||||
|
||||
|
||||
def create_mock(properties):
|
||||
mock = MagicMock()
|
||||
for property, value in properties.items():
|
||||
setattr(mock, property, value)
|
||||
return mock
|
||||
|
||||
|
||||
def setup_sm_mock():
|
||||
cruise_speed_limit = random.uniform(0, 120)
|
||||
nav_instruction_limit = random.uniform(0, 120)
|
||||
live_map_data_limit = random.uniform(0, 120)
|
||||
|
||||
cruise_state = create_mock({'speedLimit': cruise_speed_limit})
|
||||
car_state = create_mock({
|
||||
'cruiseState': cruise_state,
|
||||
'gasPressed': False,
|
||||
'brakePressed': False,
|
||||
'standstill': False,
|
||||
})
|
||||
nav_instruction = create_mock({'speedLimit': nav_instruction_limit})
|
||||
live_map_data = create_mock({
|
||||
'speedLimit': live_map_data_limit,
|
||||
'speedLimitValid': True,
|
||||
'speedLimitAhead': 0.,
|
||||
'speedLimitAheadValid': 0.,
|
||||
'speedLimitAheadDistance': 0.,
|
||||
'lastGpsTimestamp': time.time() * 1e3,
|
||||
})
|
||||
sm_mock = MagicMock()
|
||||
sm_mock.__getitem__.side_effect = lambda key: {
|
||||
'carState': car_state,
|
||||
'navInstruction': nav_instruction,
|
||||
'liveMapDataSP': live_map_data,
|
||||
}[key]
|
||||
return sm_mock
|
||||
|
||||
|
||||
parametrized_policies = pytest.mark.parametrize(
|
||||
"policy, sm_key, function_key", [
|
||||
(Policy.car_state_only, 'carState', 'car_state'),
|
||||
(Policy.car_state_priority, 'carState', 'car_state'),
|
||||
(Policy.map_data_only, 'liveMapDataSP', 'map_data'),
|
||||
(Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
|
||||
(Policy.nav_only, 'navInstruction', 'nav'),
|
||||
(Policy.nav_priority, 'navInstruction', 'nav'),
|
||||
],
|
||||
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"])
|
||||
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:
|
||||
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):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = setup_sm_mock()
|
||||
source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the resolver
|
||||
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
|
||||
assert speed_limit == source_speed_limit
|
||||
assert source == Source[function_key]
|
||||
|
||||
def test_resolver_combined(self, resolver_class):
|
||||
resolver = resolver_class(Policy.combined)
|
||||
sm_mock = setup_sm_mock()
|
||||
socket_to_source = {'carState': Source.car_state, 'liveMapDataSP': Source.map_data, 'navInstruction': Source.nav}
|
||||
minimum_key, minimum_speed_limit = min(
|
||||
((key, sm_mock[key].cruiseState.speedLimit) if key == 'carState' else (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, 0, sm_mock)
|
||||
assert speed_limit == minimum_speed_limit
|
||||
assert source == socket_to_source[minimum_key]
|
||||
|
||||
@parametrized_policies
|
||||
def test_parser(self, resolver_class, policy, sm_key, function_key):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = setup_sm_mock()
|
||||
source_speed_limit = sm_mock[sm_key].cruiseState.speedLimit if sm_key == 'carState' else sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the parsing
|
||||
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
|
||||
assert resolver._limit_solutions[Source[function_key]] == source_speed_limit
|
||||
assert resolver._distance_solutions[Source[function_key]] == 0.
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_resolve_interaction_in_update(self, resolver_class, policy):
|
||||
v_ego = 50
|
||||
resolver = resolver_class(policy)
|
||||
|
||||
sm_mock = setup_sm_mock()
|
||||
_speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock)
|
||||
|
||||
# After resolution
|
||||
assert _speed_limit is not None
|
||||
assert _distance is not None
|
||||
assert _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):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = MagicMock()
|
||||
sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
|
||||
resolver._sm = sm_mock
|
||||
resolver._get_from_map_data()
|
||||
assert resolver._limit_solutions[Source.map_data] == 0.
|
||||
assert resolver._distance_solutions[Source.map_data] == 0.
|
||||
@@ -998,9 +998,9 @@ void SpeedLimitOffsetType::refresh() {
|
||||
if (option == "0") {
|
||||
setLabel(tr("Default"));
|
||||
} else if (option == "1") {
|
||||
setLabel(tr("%"));
|
||||
} else if (option == "2") {
|
||||
setLabel(tr("Value"));
|
||||
} else if (option == "2") {
|
||||
setLabel(tr("%"));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user