diff --git a/release/files_common b/release/files_common index 9c03528164..258b41868d 100644 --- a/release/files_common +++ b/release/files_common @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index be2e6221ac..f8b496dc3d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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 diff --git a/selfdrive/controls/lib/speed_limit_controller.py b/selfdrive/controls/lib/speed_limit_controller.py deleted file mode 100644 index 8d1c0a3677..0000000000 --- a/selfdrive/controls/lib/speed_limit_controller.py +++ /dev/null @@ -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) diff --git a/selfdrive/controls/lib/sunnypilot/__init__.py b/selfdrive/controls/lib/sunnypilot/__init__.py new file mode 100644 index 0000000000..27aef8996f --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/__init__.py @@ -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 + diff --git a/selfdrive/controls/lib/sunnypilot/common.py b/selfdrive/controls/lib/sunnypilot/common.py new file mode 100644 index 0000000000..95090445e0 --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/common.py @@ -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 diff --git a/selfdrive/controls/lib/sunnypilot/helpers.py b/selfdrive/controls/lib/sunnypilot/helpers.py new file mode 100644 index 0000000000..d7be1bf536 --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/helpers.py @@ -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' diff --git a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py b/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py new file mode 100644 index 0000000000..0b66ec2a2b --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py @@ -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) diff --git a/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py b/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py new file mode 100644 index 0000000000..5617239fcd --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py @@ -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 diff --git a/selfdrive/controls/lib/sunnypilot/tests/test_speed_limit_resolver.py b/selfdrive/controls/lib/sunnypilot/tests/test_speed_limit_resolver.py new file mode 100644 index 0000000000..2903f98c23 --- /dev/null +++ b/selfdrive/controls/lib/sunnypilot/tests/test_speed_limit_resolver.py @@ -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. diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc index 07b234ac45..e24302103f 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc @@ -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("%")); } }