From 4b8109666b9bf52741dd2980ab72efb2d86baecb Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Mon, 23 Oct 2023 11:05:17 +0000 Subject: [PATCH] Revert "Refactoring the speed limit control" --- release/files_common | 6 +- .../controls/lib/longitudinal_planner.py | 5 +- .../controls/lib/speed_limit_controller.py | 428 ++++++++++++++++++ selfdrive/controls/lib/sunnypilot/__init__.py | 13 - selfdrive/controls/lib/sunnypilot/common.py | 18 - selfdrive/controls/lib/sunnypilot/helpers.py | 19 - .../lib/sunnypilot/speed_limit_controller.py | 238 ---------- .../lib/sunnypilot/speed_limit_resolver.py | 151 ------ 8 files changed, 431 insertions(+), 447 deletions(-) create mode 100644 selfdrive/controls/lib/speed_limit_controller.py delete mode 100644 selfdrive/controls/lib/sunnypilot/__init__.py delete mode 100644 selfdrive/controls/lib/sunnypilot/common.py delete mode 100644 selfdrive/controls/lib/sunnypilot/helpers.py delete mode 100644 selfdrive/controls/lib/sunnypilot/speed_limit_controller.py delete mode 100644 selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py diff --git a/release/files_common b/release/files_common index b4cbd8e373..61a755323a 100644 --- a/release/files_common +++ b/release/files_common @@ -213,14 +213,10 @@ 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 ec0b898b8c..5511970ca4 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,8 +9,6 @@ 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 T_IDXS from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState @@ -18,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi 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 @@ -226,7 +225,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 (Source.none, Source.nav)) + longitudinalPlanSP.isMapSpeedLimit = bool(self.speed_limit_controller.source not in (SpeedLimitResolver.Source.none, SpeedLimitResolver.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 new file mode 100644 index 0000000000..14892d8add --- /dev/null +++ b/selfdrive/controls/lib/speed_limit_controller.py @@ -0,0 +1,428 @@ +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 selfdrive.modeld.constants import T_IDXS + + +_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 / 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 / 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 deleted file mode 100644 index 27aef8996f..0000000000 --- a/selfdrive/controls/lib/sunnypilot/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -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 deleted file mode 100644 index 95090445e0..0000000000 --- a/selfdrive/controls/lib/sunnypilot/common.py +++ /dev/null @@ -1,18 +0,0 @@ -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 deleted file mode 100644 index d7be1bf536..0000000000 --- a/selfdrive/controls/lib/sunnypilot/helpers.py +++ /dev/null @@ -1,19 +0,0 @@ -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 deleted file mode 100644 index 1a3812e776..0000000000 --- a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py +++ /dev/null @@ -1,238 +0,0 @@ -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 T_IDXS - - -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 = 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_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 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 / 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 / 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 deleted file mode 100644 index c84166d9e6..0000000000 --- a/selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py +++ /dev/null @@ -1,151 +0,0 @@ -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.map_data], - Policy.map_data_priority: [Source.map_data, 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 next_speed_limit > 0. and 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): - solutions = self._get_solutions_according_to_policy() - limits = solutions['limits'] - distances = solutions['distances'] - sources = solutions['sources'] - extra_sources = [] - - # 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 = Source(sources[min_idx]) - elif self._policy in [Policy.car_state_priority, Policy.nav_priority, Policy.map_data_priority]: - # If policy is car_state_priority -> append map data - if self._policy == Policy.car_state_priority: - extra_sources = [Source.map_data] - # If policy is map_data_priority --> append car state - elif self._policy == Policy.map_data_priority: - extra_sources = [Source.car_state] - # If policy is nav_priority -> append map data then car state - elif self._policy == Policy.nav_priority: - extra_sources = [Source.map_data, Source.car_state] - - for src in extra_sources: - if self._limit_solutions[src] > 0: - # If the speed limit from the source is above 0, then choose this source - self.speed_limit = self._limit_solutions[src] - self.distance = self._distance_solutions[src] - self.source = src - break - else: - self.speed_limit = 0. - self.distance = 0. - self.source = Source.none - - else: - self.speed_limit = 0. - self.distance = 0. - self.source = 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_solutions_according_to_policy(self): - source_codes = self._policy_to_sources_map[self._policy] - limits = np.array([self._limit_solutions[source] for source in source_codes], dtype=float) - distances = np.array([self._distance_solutions[source] for source in source_codes], dtype=float) - sources = np.array([source.value for source in source_codes], dtype=int) - return {'limits': limits, 'distances': distances, 'sources': sources}