mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-18 15:23:57 +08:00
remove old mapd, sla, ssc
This commit is contained in:
rav4kumar
@@ -217,7 +217,6 @@ class Car:
|
||||
if can_rcv_valid and REPLAY:
|
||||
self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
|
||||
|
||||
self.v_cruise_helper.update_speed_limit_assist(self.is_metric, self.sm['longitudinalPlanSP'])
|
||||
self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
|
||||
if self.sm['carControl'].enabled and not self.CC_prev.enabled:
|
||||
# Use CarState w/ buttons from the step selfdrived enables on
|
||||
|
||||
@@ -53,7 +53,6 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
|
||||
# if stock cruise is completely disabled, then we can use our own set speed logic
|
||||
self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
|
||||
self.update_speed_limit_assist_v_cruise_non_pcm()
|
||||
self.v_cruise_cluster_kph = self.v_cruise_kph
|
||||
self.update_button_timers(CS, enabled)
|
||||
else:
|
||||
@@ -105,12 +104,6 @@ class VCruiseHelper(VCruiseHelperSP):
|
||||
if not self.button_change_states[button_type]["enabled"]:
|
||||
return
|
||||
|
||||
# Speed Limit Assist for Non PCM long cars.
|
||||
# True: Disallow set speed changes when user confirmed the target set speed during preActive state
|
||||
# False: Allow set speed changes as SLA is not requesting user confirmation
|
||||
if self.update_speed_limit_assist_pre_active_confirmed(button_type):
|
||||
return
|
||||
|
||||
long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
|
||||
if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval
|
||||
self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
|
||||
|
||||
@@ -17,7 +17,7 @@ class UIStateSP:
|
||||
self.params = Params()
|
||||
self.sm_services_ext = [
|
||||
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
|
||||
"gpsLocation", "liveTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "liveDelay"
|
||||
"gpsLocation", "liveTorqueParameters", "carStateSP", "carParamsSP", "liveDelay"
|
||||
]
|
||||
|
||||
self.sunnylink_state = SunnylinkState()
|
||||
|
||||
@@ -1,5 +0,0 @@
|
||||
import os
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
|
||||
MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj')
|
||||
MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')
|
||||
@@ -1,22 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
LOOK_AHEAD_HORIZON_TIME = 15. # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
|
||||
_DEBUG = False
|
||||
_CLOUDLOG_DEBUG = False
|
||||
ROAD_NAME_TIMEOUT = 30 # secs
|
||||
R = 6373000.0 # approximate radius of earth in mts
|
||||
QUERY_RADIUS = 3000 # mts. Radius to use on OSM data queries.
|
||||
QUERY_RADIUS_OFFLINE = 2250 # mts. Radius to use on offline OSM data queries.
|
||||
|
||||
|
||||
def get_debug(msg, log_to_cloud=True):
|
||||
if _CLOUDLOG_DEBUG and log_to_cloud:
|
||||
cloudlog.debug(msg)
|
||||
if _DEBUG:
|
||||
print(msg)
|
||||
@@ -1,65 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.navd.helpers import coordinate_from_param
|
||||
|
||||
MAX_SPEED_LIMIT = V_CRUISE_UNSET * CV.KPH_TO_MS
|
||||
|
||||
|
||||
class BaseMapData(ABC):
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
|
||||
self.sm = messaging.SubMaster(['liveLocationKalman'])
|
||||
self.pm = messaging.PubMaster(['liveMapDataSP'])
|
||||
|
||||
self.localizer_valid = False
|
||||
self.last_bearing = None
|
||||
self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
|
||||
|
||||
@abstractmethod
|
||||
def update_location(self) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_current_speed_limit(self) -> float:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_current_road_name(self) -> str:
|
||||
pass
|
||||
|
||||
def publish(self) -> None:
|
||||
speed_limit = self.get_current_speed_limit()
|
||||
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
|
||||
|
||||
mapd_sp_send = messaging.new_message('liveMapDataSP')
|
||||
mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
|
||||
live_map_data = mapd_sp_send.liveMapDataSP
|
||||
|
||||
live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
|
||||
live_map_data.speedLimit = speed_limit
|
||||
live_map_data.speedLimitAheadValid = bool(MAX_SPEED_LIMIT > next_speed_limit > 0)
|
||||
live_map_data.speedLimitAhead = next_speed_limit
|
||||
live_map_data.speedLimitAheadDistance = next_speed_limit_distance
|
||||
live_map_data.roadName = self.get_current_road_name()
|
||||
|
||||
self.pm.send('liveMapDataSP', mapd_sp_send)
|
||||
|
||||
def tick(self) -> None:
|
||||
self.sm.update(0)
|
||||
self.update_location()
|
||||
self.publish()
|
||||
@@ -1,56 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# DISCLAIMER: This code is intended principally for development and debugging purposes.
|
||||
# Although it provides a standalone entry point to the program, users should refer
|
||||
# to the actual implementations for consumption. Usage outside of development scenarios
|
||||
# is not advised and could lead to unpredictable results.
|
||||
|
||||
import threading
|
||||
import traceback
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
|
||||
from openpilot.sunnypilot.mapd.live_map_data import get_debug
|
||||
|
||||
|
||||
def excepthook(args):
|
||||
get_debug(f'MapD: Threading exception:\n{args}')
|
||||
traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
|
||||
|
||||
|
||||
def live_map_data_sp_thread():
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
params = Params()
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
|
||||
while True:
|
||||
live_map_data_sp_thread_debug(gps_location_service)
|
||||
|
||||
|
||||
def live_map_data_sp_thread_debug(gps_location_service):
|
||||
_sub_master = messaging.SubMaster(['carState', 'livePose', 'liveMapDataSP', 'longitudinalPlanSP', 'carStateSP', gps_location_service])
|
||||
_sub_master.update()
|
||||
|
||||
v_ego = _sub_master['carState'].vEgo
|
||||
_resolver = SpeedLimitResolver()
|
||||
_resolver.policy = Policy.car_state_priority
|
||||
_resolver.update(v_ego, _sub_master)
|
||||
print(_resolver.speed_limit, _resolver.distance, _resolver.source)
|
||||
|
||||
|
||||
def main():
|
||||
threading.excepthook = excepthook
|
||||
live_map_data_sp_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,61 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
from cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate
|
||||
|
||||
|
||||
class OsmMapData(BaseMapData):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
|
||||
def update_location(self) -> None:
|
||||
location = self.sm['liveLocationKalman']
|
||||
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
|
||||
if self.localizer_valid:
|
||||
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
|
||||
|
||||
if self.last_position is None:
|
||||
return
|
||||
|
||||
params = {
|
||||
"latitude": self.last_position.latitude,
|
||||
"longitude": self.last_position.longitude,
|
||||
}
|
||||
|
||||
if self.last_bearing is not None:
|
||||
params['bearing'] = self.last_bearing
|
||||
|
||||
self.mem_params.put("LastGPSPosition", json.dumps(params))
|
||||
|
||||
def get_current_speed_limit(self) -> float:
|
||||
return float(self.mem_params.get("MapSpeedLimit") or 0.0)
|
||||
|
||||
def get_current_road_name(self) -> str:
|
||||
return str(self.mem_params.get("RoadName") or "")
|
||||
|
||||
def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
|
||||
next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit")
|
||||
next_speed_limit_section = next_speed_limit_section_str if next_speed_limit_section_str else {}
|
||||
next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
|
||||
next_speed_limit_latitude = next_speed_limit_section.get('latitude')
|
||||
next_speed_limit_longitude = next_speed_limit_section.get('longitude')
|
||||
next_speed_limit_distance = 0.0
|
||||
|
||||
if next_speed_limit_latitude and next_speed_limit_longitude:
|
||||
next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
|
||||
next_speed_limit_distance = (self.last_position or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
|
||||
|
||||
return next_speed_limit, next_speed_limit_distance
|
||||
@@ -1,42 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# DISCLAIMER: This code is intended principally for development and debugging purposes.
|
||||
# Although it provides a standalone entry point to the program, users should refer
|
||||
# to the actual implementations for consumption. Usage outside of development scenarios
|
||||
# is not advised and could lead to unpredictable results.
|
||||
|
||||
import threading
|
||||
import traceback
|
||||
|
||||
from openpilot.common.realtime import Ratekeeper, config_realtime_process
|
||||
from openpilot.sunnypilot.mapd.live_map_data import get_debug
|
||||
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
|
||||
|
||||
|
||||
def excepthook(args):
|
||||
get_debug(f'MapD: Threading exception:\n{args}')
|
||||
traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
|
||||
|
||||
|
||||
def live_map_data_sp_thread():
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
live_map_sp = OsmMapData()
|
||||
rk = Ratekeeper(1, print_delay_threshold=None)
|
||||
|
||||
while True:
|
||||
live_map_sp.tick()
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def main():
|
||||
threading.excepthook = excepthook
|
||||
live_map_data_sp_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,154 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import logging
|
||||
import os
|
||||
import stat
|
||||
import time
|
||||
import traceback
|
||||
import requests
|
||||
from pathlib import Path
|
||||
from urllib.request import urlopen
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.common.spinner import Spinner
|
||||
from openpilot.system.version import is_prebuilt
|
||||
from openpilot.sunnypilot.mapd import MAPD_PATH, MAPD_BIN_DIR
|
||||
import openpilot.system.sentry as sentry
|
||||
|
||||
VERSION = "v1.12.0"
|
||||
URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
|
||||
|
||||
|
||||
def update_installed_version(version: str, params: Params = None) -> None:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
params.put("MapdVersion", version)
|
||||
|
||||
|
||||
class MapdInstallManager:
|
||||
def __init__(self, spinner_ref: Spinner):
|
||||
self._spinner = spinner_ref
|
||||
self._params = Params()
|
||||
|
||||
def download(self) -> None:
|
||||
self.ensure_directories_exist()
|
||||
self._download_file()
|
||||
update_installed_version(VERSION, self._params)
|
||||
|
||||
def check_and_download(self) -> None:
|
||||
if self.download_needed():
|
||||
self.download()
|
||||
|
||||
def download_needed(self) -> bool:
|
||||
return not os.path.exists(MAPD_PATH) or self.get_installed_version() != VERSION
|
||||
|
||||
@staticmethod
|
||||
def ensure_directories_exist() -> None:
|
||||
if not os.path.exists(Paths.mapd_root()):
|
||||
os.makedirs(Paths.mapd_root())
|
||||
if not os.path.exists(MAPD_BIN_DIR):
|
||||
os.makedirs(MAPD_BIN_DIR)
|
||||
|
||||
@staticmethod
|
||||
def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None:
|
||||
with open(file_path, 'wb') as output:
|
||||
output.write(content)
|
||||
output.flush()
|
||||
os.fsync(output.fileno())
|
||||
current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode)
|
||||
os.chmod(file_path, current_permissions | stat.S_IEXEC)
|
||||
|
||||
def _download_file(self, num_retries=5) -> None:
|
||||
temp_file = Path(MAPD_PATH + ".tmp")
|
||||
download_timeout = 60
|
||||
for cnt in range(num_retries):
|
||||
try:
|
||||
response = requests.get(URL, stream=True, timeout=download_timeout)
|
||||
response.raise_for_status()
|
||||
self._safe_write_and_set_executable(temp_file, response.content)
|
||||
# No exceptions encountered. Safe to replace original file.
|
||||
temp_file.replace(MAPD_PATH)
|
||||
return
|
||||
except requests.exceptions.ReadTimeout:
|
||||
self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]")
|
||||
time.sleep(0.5)
|
||||
except requests.exceptions.RequestException as e:
|
||||
self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]")
|
||||
time.sleep(0.5)
|
||||
|
||||
# Delete temp file if the process was not successful.
|
||||
if temp_file.exists():
|
||||
temp_file.unlink()
|
||||
logging.error("Failed to download file after all retries")
|
||||
|
||||
def get_installed_version(self) -> str:
|
||||
return str(self._params.get("MapdVersion") or "")
|
||||
|
||||
def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
|
||||
max_retries = 10
|
||||
for retries in range(max_retries + 1):
|
||||
self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
|
||||
time.sleep(2)
|
||||
try:
|
||||
_ = urlopen('https://sentry.io', timeout=10)
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f'Wait for internet failed: {e}')
|
||||
if return_on_failure and retries == max_retries:
|
||||
return False
|
||||
|
||||
return False
|
||||
|
||||
def non_prebuilt_install(self) -> None:
|
||||
sm = messaging.SubMaster(['deviceState'])
|
||||
metered = sm['deviceState'].networkMetered
|
||||
|
||||
if metered:
|
||||
self._spinner.update("Can't proceed with mapd install since network is metered!")
|
||||
time.sleep(5)
|
||||
return
|
||||
|
||||
try:
|
||||
self.ensure_directories_exist()
|
||||
if not self.download_needed():
|
||||
self._spinner.update("Mapd is good!")
|
||||
time.sleep(0.1)
|
||||
return
|
||||
|
||||
if self.wait_for_internet_connection(return_on_failure=True):
|
||||
self._spinner.update(f"Downloading pfeiferj's mapd [{self.get_installed_version()}] => [{VERSION}].")
|
||||
time.sleep(0.1)
|
||||
self.check_and_download()
|
||||
self._spinner.close()
|
||||
|
||||
except Exception:
|
||||
for i in range(6):
|
||||
self._spinner.update("Failed to download OSM maps won't work until properly downloaded!" +
|
||||
"Try again manually rebooting. " +
|
||||
f"Boot will continue in {5 - i}s...")
|
||||
time.sleep(1)
|
||||
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
traceback.print_exc()
|
||||
sentry.capture_exception()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
spinner = Spinner()
|
||||
install_manager = MapdInstallManager(spinner)
|
||||
install_manager.ensure_directories_exist()
|
||||
if is_prebuilt():
|
||||
debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]"
|
||||
spinner.update(debug_msg)
|
||||
update_installed_version(VERSION)
|
||||
else:
|
||||
spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
|
||||
install_manager.non_prebuilt_install()
|
||||
@@ -1,144 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import platform
|
||||
import os
|
||||
import glob
|
||||
import shutil
|
||||
from datetime import datetime
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Ratekeeper, config_realtime_process
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.sunnypilot.mapd import MAPD_PATH
|
||||
from openpilot.sunnypilot.mapd.mapd_installer import VERSION, update_installed_version
|
||||
|
||||
# PFEIFER - MAPD {{
|
||||
params = Params()
|
||||
mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
|
||||
# }} PFEIFER - MAPD
|
||||
|
||||
|
||||
def get_files_for_cleanup() -> list[str]:
|
||||
paths = [
|
||||
f"{Paths.mapd_root()}/db",
|
||||
f"{Paths.mapd_root()}/v*"
|
||||
]
|
||||
files_to_remove = []
|
||||
for path in paths:
|
||||
if os.path.exists(path):
|
||||
files = glob.glob(path + '/**', recursive=True)
|
||||
files_to_remove.extend(files)
|
||||
# check for version and mapd files
|
||||
if not os.path.isfile(MAPD_PATH):
|
||||
files_to_remove.append(MAPD_PATH)
|
||||
return files_to_remove
|
||||
|
||||
|
||||
def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
|
||||
for file in files_to_remove:
|
||||
# Remove trailing slash if path is file
|
||||
if file.endswith('/') and os.path.isfile(file[:-1]):
|
||||
file = file[:-1]
|
||||
# Try to remove as file or symbolic link first
|
||||
if os.path.islink(file) or os.path.isfile(file):
|
||||
os.remove(file)
|
||||
elif os.path.isdir(file): # If it's a directory
|
||||
shutil.rmtree(file, ignore_errors=False)
|
||||
|
||||
|
||||
def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None:
|
||||
params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
|
||||
params.put_bool("OsmDbUpdatesCheck", False)
|
||||
|
||||
osm_download_locations = {
|
||||
"nations": nations,
|
||||
"states": states or []
|
||||
}
|
||||
|
||||
print(f"Downloading maps for {json.dumps(osm_download_locations)}")
|
||||
mem_params.put("OSMDownloadLocations", osm_download_locations)
|
||||
|
||||
|
||||
def filter_nations_and_states(nations: list[str], states: list[str] = None) -> tuple[list[str], list[str]]:
|
||||
"""Filters and prepares nation and state data for OSM map download.
|
||||
|
||||
If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list.
|
||||
If the nation is 'US' and the state is 'All', the 'All' is removed from the list.
|
||||
The idea behind these filters is that if a specific state in the US is provided,
|
||||
there's no need to download map data for the entire US. Conversely,
|
||||
if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US,
|
||||
and 'All' isn't a valid state name, so it's removed.
|
||||
|
||||
Parameters:
|
||||
nations (list): A list of nations for which the map data is to be downloaded.
|
||||
states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None.
|
||||
|
||||
Returns:
|
||||
tuple: Two lists. The first list is filtered nations and the second list is filtered states.
|
||||
"""
|
||||
|
||||
if "US" in nations and states and not any(x.lower() == "all" for x in states):
|
||||
# If a specific state in the US is provided, remove 'US' from nations
|
||||
nations.remove("US")
|
||||
elif "US" in nations and states and any(x.lower() == "all" for x in states):
|
||||
# If 'All' is provided as a state (case invariant), remove those instances from states
|
||||
states = [x for x in states if x.lower() != "all"]
|
||||
elif "US" not in nations and states and any(x.lower() == "all" for x in states):
|
||||
states.remove("All")
|
||||
return nations, states or []
|
||||
|
||||
|
||||
def update_osm_db() -> None:
|
||||
if params.get_bool("OsmDbUpdatesCheck"):
|
||||
cleanup_old_osm_data(get_files_for_cleanup())
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
state = params.get("OsmStateName", return_default=True)
|
||||
filtered_nations, filtered_states = filter_nations_and_states([country], [state])
|
||||
request_refresh_osm_location_data(filtered_nations, filtered_states)
|
||||
|
||||
if not mem_params.get("OSMDownloadBounds"):
|
||||
mem_params.put("OSMDownloadBounds", "")
|
||||
|
||||
if not mem_params.get("LastGPSPosition"):
|
||||
mem_params.put("LastGPSPosition", "{}")
|
||||
|
||||
|
||||
def main_thread():
|
||||
update_installed_version(VERSION, params)
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
rk = Ratekeeper(1, print_delay_threshold=None)
|
||||
live_map_sp = OsmMapData()
|
||||
|
||||
# Create folder needed for OSM
|
||||
try:
|
||||
os.mkdir(Paths.mapd_root())
|
||||
except FileExistsError:
|
||||
pass
|
||||
except PermissionError:
|
||||
cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}")
|
||||
|
||||
while True:
|
||||
show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
|
||||
set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
|
||||
|
||||
update_osm_db()
|
||||
live_map_sp.tick()
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def main():
|
||||
main_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1 +0,0 @@
|
||||
fdb3b49ee19956e6ce09fdc3373cbba557f1263b2180e9f344c1d4053852284b
|
||||
@@ -1,19 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.sunnypilot import get_file_hash
|
||||
from openpilot.sunnypilot.mapd import MAPD_PATH
|
||||
from openpilot.sunnypilot.mapd.update_version import MAPD_HASH_PATH
|
||||
|
||||
|
||||
class TestMapdVersion:
|
||||
def test_compare_versions(self):
|
||||
mapd_hash = get_file_hash(MAPD_PATH)
|
||||
|
||||
with open(MAPD_HASH_PATH) as f:
|
||||
current_hash = f.read().strip()
|
||||
|
||||
assert current_hash == mapd_hash, "Run sunnypilot/mapd/update_version.py to update the current mapd version and hash"
|
||||
@@ -1,97 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import re
|
||||
|
||||
from openpilot.sunnypilot import get_file_hash
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.mapd import MAPD_PATH
|
||||
|
||||
MAPD_HASH_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "tests", "mapd_hash")
|
||||
MAPD_VERSION_PATH = os.path.join(BASEDIR, "sunnypilot", "mapd", "mapd_installer.py")
|
||||
|
||||
|
||||
def update_mapd_hash():
|
||||
mapd_hash = get_file_hash(MAPD_PATH)
|
||||
|
||||
with open(MAPD_HASH_PATH, "w") as f:
|
||||
f.write(mapd_hash)
|
||||
|
||||
print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}")
|
||||
|
||||
|
||||
def get_current_mapd_version(path: str) -> str:
|
||||
print("[GET CURRENT MAPD VERSION]")
|
||||
with open(path) as f:
|
||||
for line in f:
|
||||
if line.strip().startswith("VERSION"):
|
||||
# Match VERSION = 'v1.11.0' or VERSION="v1.11.0" (with optional spaces)
|
||||
match = re.search(r'VERSION\s*=\s*[\'"]([^\'"]+)[\'"]', line)
|
||||
if match:
|
||||
ver = match.group(1)
|
||||
print(f'Current mapd version: "{ver}"')
|
||||
return ver
|
||||
else:
|
||||
print("[ERROR] VERSION line found but no quoted value detected.")
|
||||
return ""
|
||||
print("[ERROR] VERSION not found in file!")
|
||||
return ""
|
||||
|
||||
|
||||
def update_mapd_version(ver: str, path: str):
|
||||
print("[CHANGE CURRENT MAPD VERSION]")
|
||||
|
||||
with open(path) as f:
|
||||
lines = f.readlines()
|
||||
|
||||
found = False
|
||||
new_lines = []
|
||||
for line in lines:
|
||||
if not found and line.startswith("VERSION ="):
|
||||
new_lines.append(f'VERSION = "{ver}"\n')
|
||||
found = True
|
||||
new_lines.extend(lines[lines.index(line) + 1:])
|
||||
break
|
||||
else:
|
||||
new_lines.append(line)
|
||||
|
||||
if not found:
|
||||
print("[ERROR] VERSION line not found! Aborting without writing.")
|
||||
return
|
||||
|
||||
with open(path, "w") as f:
|
||||
f.writelines(new_lines)
|
||||
|
||||
print(f'New mapd version: "{ver}"')
|
||||
print("[DONE]")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Update mapd version and hash")
|
||||
parser.add_argument("--new_ver", type=str, help="New mapd version")
|
||||
args = parser.parse_args()
|
||||
|
||||
if not args.new_ver:
|
||||
print("Warning: No new mapd version provided. Use --new_ver to specify")
|
||||
print("Example:")
|
||||
print(" python sunnypilot/mapd/update_version.py --new_ver \"v1.12.0\"")
|
||||
print("Current mapd version and hash will not be updated! (aborted)")
|
||||
exit(0)
|
||||
|
||||
current_ver = get_current_mapd_version(MAPD_VERSION_PATH)
|
||||
new_ver = f"{args.new_ver}"
|
||||
if current_ver == new_ver:
|
||||
print(f'Proposed mapd version: "{new_ver}"')
|
||||
confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip()
|
||||
if confirm != "Y":
|
||||
print("Current mapd version and hash will not be updated! (aborted)")
|
||||
exit(0)
|
||||
|
||||
update_mapd_version(new_ver, MAPD_VERSION_PATH)
|
||||
update_mapd_hash()
|
||||
@@ -6,16 +6,12 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from cereal import car, custom
|
||||
from cereal import car
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
|
||||
ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
|
||||
@@ -54,16 +50,6 @@ class VCruiseHelperSP:
|
||||
|
||||
self.enable_button_timers = CRUISE_BUTTON_TIMER
|
||||
|
||||
# Speed Limit Assist
|
||||
self.sla_state = SpeedLimitAssistState.disabled
|
||||
self.prev_sla_state = SpeedLimitAssistState.disabled
|
||||
self.has_speed_limit = False
|
||||
self.speed_limit_final_last = 0.
|
||||
self.speed_limit_final_last_kph = 0.
|
||||
self.prev_speed_limit_final_last_kph = 0.
|
||||
self.req_plus = False
|
||||
self.req_minus = False
|
||||
|
||||
def read_custom_set_speed_params(self) -> None:
|
||||
self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
|
||||
self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
|
||||
@@ -106,33 +92,3 @@ class VCruiseHelperSP:
|
||||
return enabled and self.enabled_prev
|
||||
|
||||
return enabled
|
||||
|
||||
def update_speed_limit_assist(self, is_metric, LP_SP: custom.LongitudinalPlanSP) -> None:
|
||||
resolver = LP_SP.speedLimit.resolver
|
||||
self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
|
||||
self.speed_limit_final_last = LP_SP.speedLimit.resolver.speedLimitFinalLast
|
||||
self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
|
||||
self.sla_state = LP_SP.speedLimit.assist.state
|
||||
self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
|
||||
self.speed_limit_final_last, is_metric)
|
||||
|
||||
@property
|
||||
def update_speed_limit_final_last_changed(self) -> bool:
|
||||
return self.has_speed_limit and bool(self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph)
|
||||
|
||||
def update_speed_limit_assist_pre_active_confirmed(self, button_type: car.CarState.ButtonEvent.Type) -> bool:
|
||||
if self.sla_state == SpeedLimitAssistState.preActive or self.prev_sla_state == SpeedLimitAssistState.preActive:
|
||||
if button_type == ButtonType.decelCruise and self.req_minus:
|
||||
return True
|
||||
if button_type == ButtonType.accelCruise and self.req_plus:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
|
||||
if self.sla_state in SLA_ACTIVE_STATES and (self.prev_sla_state not in SLA_ACTIVE_STATES or
|
||||
self.update_speed_limit_final_last_changed):
|
||||
self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
|
||||
|
||||
self.prev_sla_state = self.sla_state
|
||||
self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
|
||||
|
||||
@@ -11,7 +11,6 @@ from opendbc.car.interfaces import CarInterfaceBase
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
|
||||
|
||||
import openpilot.system.sentry as sentry
|
||||
|
||||
@@ -86,10 +85,6 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS
|
||||
cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
|
||||
params.remove("DynamicExperimentalControl")
|
||||
params.remove("CustomAccIncrementsEnabled")
|
||||
params.remove("SmartCruiseControlVision")
|
||||
params.remove("SmartCruiseControlMap")
|
||||
|
||||
set_speed_limit_assist_availability(CP, CP_SP, params)
|
||||
|
||||
|
||||
def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
|
||||
|
||||
@@ -1,9 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.common.constants import CV
|
||||
|
||||
MIN_V = 20 * CV.KPH_TO_MS # Do not operate under 20 km/h
|
||||
@@ -1,261 +0,0 @@
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.navd.helpers import coordinate_from_param, Coordinate
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
ACTIVE_STATES = (MapState.turning, )
|
||||
ENABLED_STATES = (MapState.enabled, MapState.overriding, *ACTIVE_STATES)
|
||||
|
||||
R = 6373000.0 # approximate radius of earth in meters
|
||||
TO_RADIANS = math.pi / 180
|
||||
TO_DEGREES = 180 / math.pi
|
||||
TARGET_JERK = -0.6 # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
|
||||
TARGET_ACCEL = -1.2 # m/s^2 should match up with the long planner limit
|
||||
TARGET_OFFSET = 1.0 # seconds - This controls how soon before the curve you reach the target velocity. It also helps
|
||||
# reach the target velocity when inaccuracies in the distance modeling logic would cause overshoot.
|
||||
# The value is multiplied against the target velocity to determine the additional distance. This is
|
||||
# done to keep the distance calculations consistent but results in the offset actually being less
|
||||
# time than specified depending on how much of a speed differential there is between v_ego and the
|
||||
# target velocity.
|
||||
|
||||
|
||||
def velocities_from_param(param: str, params: Params):
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
json_str = params.get(param)
|
||||
if json_str is None:
|
||||
return None
|
||||
|
||||
velocities = json.loads(json_str)
|
||||
|
||||
return velocities
|
||||
|
||||
|
||||
def calculate_accel(t, target_jerk, a_ego):
|
||||
return a_ego + target_jerk * t
|
||||
|
||||
|
||||
def calculate_velocity(t, target_jerk, a_ego, v_ego):
|
||||
return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
|
||||
|
||||
|
||||
def calculate_distance(t, target_jerk, a_ego, v_ego):
|
||||
return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
|
||||
|
||||
|
||||
# points should be in radians
|
||||
# output is meters
|
||||
def distance_to_point(ax, ay, bx, by):
|
||||
a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
|
||||
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
|
||||
|
||||
return R * c # in meters
|
||||
|
||||
|
||||
class SmartCruiseControlMap:
|
||||
v_target: float = 0
|
||||
a_target: float = 0.
|
||||
v_ego: float = 0.
|
||||
a_ego: float = 0.
|
||||
output_v_target: float = V_CRUISE_UNSET
|
||||
output_a_target: float = 0.
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
self.long_enabled = False
|
||||
self.long_override = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.state = MapState.disabled
|
||||
self.v_cruise = 0
|
||||
self.target_lat = 0.0
|
||||
self.target_lon = 0.0
|
||||
self.frame = -1
|
||||
|
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
|
||||
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self.is_active:
|
||||
return max(self.v_target, MIN_V)
|
||||
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def update_params(self):
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
|
||||
def update_calculations(self) -> None:
|
||||
self.last_position = coordinate_from_param("LastGPSPosition", self.mem_params) or Coordinate(0.0, 0.0)
|
||||
lat = self.last_position.latitude
|
||||
lon = self.last_position.longitude
|
||||
|
||||
self.target_velocities = velocities_from_param("MapTargetVelocities", self.mem_params) or []
|
||||
|
||||
if self.last_position is None or self.target_velocities is None:
|
||||
return
|
||||
|
||||
min_dist = 1000
|
||||
min_idx = 0
|
||||
distances = []
|
||||
|
||||
# find our location in the path
|
||||
for i in range(len(self.target_velocities)):
|
||||
target_velocity = self.target_velocities[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
|
||||
distances.append(d)
|
||||
if d < min_dist:
|
||||
min_dist = d
|
||||
min_idx = i
|
||||
|
||||
# only look at values from our current position forward
|
||||
forward_points = self.target_velocities[min_idx:]
|
||||
forward_distances = distances[min_idx:]
|
||||
|
||||
# find velocities that we are within the distance we need to adjust for
|
||||
valid_velocities = []
|
||||
for i in range(len(forward_points)):
|
||||
target_velocity = forward_points[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
tv = target_velocity["velocity"]
|
||||
if tv > self.v_ego:
|
||||
continue
|
||||
|
||||
d = forward_distances[i]
|
||||
|
||||
a_diff = (self.a_ego - TARGET_ACCEL)
|
||||
accel_t = abs(a_diff / TARGET_JERK)
|
||||
min_accel_v = calculate_velocity(accel_t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
|
||||
max_d = 0
|
||||
if tv > min_accel_v:
|
||||
# calculate time needed based on target jerk
|
||||
a = 0.5 * TARGET_JERK
|
||||
b = self.a_ego
|
||||
c = self.v_ego - tv
|
||||
t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
|
||||
t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
|
||||
if not isinstance(t_a, complex) and t_a > 0:
|
||||
t = t_a
|
||||
else:
|
||||
t = t_b
|
||||
if isinstance(t, complex):
|
||||
continue
|
||||
|
||||
max_d = max_d + calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
else:
|
||||
t = accel_t
|
||||
max_d = calculate_distance(t, TARGET_JERK, self.a_ego, self.v_ego)
|
||||
|
||||
# calculate additional time needed based on target accel
|
||||
t = abs((min_accel_v - tv) / TARGET_ACCEL)
|
||||
max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
|
||||
|
||||
if d < max_d + tv * TARGET_OFFSET:
|
||||
valid_velocities.append((float(tv), tlat, tlon))
|
||||
|
||||
# Find the smallest velocity we need to adjust for
|
||||
min_v = 100.0
|
||||
target_lat = 0.0
|
||||
target_lon = 0.0
|
||||
for tv, lat, lon in valid_velocities:
|
||||
if tv < min_v:
|
||||
min_v = tv
|
||||
target_lat = lat
|
||||
target_lon = lon
|
||||
|
||||
if self.v_target < min_v and not (self.target_lat == 0 and self.target_lon == 0):
|
||||
for i in range(len(forward_points)):
|
||||
target_velocity = forward_points[i]
|
||||
tlat = target_velocity["latitude"]
|
||||
tlon = target_velocity["longitude"]
|
||||
tv = target_velocity["velocity"]
|
||||
if tv > self.v_ego:
|
||||
continue
|
||||
|
||||
if tlat == self.target_lat and tlon == self.target_lon and tv == self.v_target:
|
||||
return
|
||||
|
||||
# not found so let's reset
|
||||
self.v_target = 0.0
|
||||
self.target_lat = 0.0
|
||||
self.target_lon = 0.0
|
||||
|
||||
self.v_target = min_v
|
||||
self.target_lat = target_lat
|
||||
self.target_lon = target_lon
|
||||
|
||||
def _update_state_machine(self) -> tuple[bool, bool]:
|
||||
# ENABLED, TURNING
|
||||
if self.state != MapState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = MapState.disabled
|
||||
elif self.long_override:
|
||||
self.state = MapState.overriding
|
||||
|
||||
else:
|
||||
# ENABLED
|
||||
if self.state == MapState.enabled:
|
||||
if self.v_cruise > self.v_target != 0:
|
||||
self.state = MapState.turning
|
||||
|
||||
# TURNING
|
||||
elif self.state == MapState.turning:
|
||||
if self.v_cruise <= self.v_target or self.v_target == 0:
|
||||
self.state = MapState.enabled
|
||||
|
||||
# OVERRIDING
|
||||
elif self.state == MapState.overriding:
|
||||
if not self.long_override:
|
||||
if self.v_cruise > self.v_target != 0:
|
||||
self.state = MapState.turning
|
||||
else:
|
||||
self.state = MapState.enabled
|
||||
|
||||
# DISABLED
|
||||
elif self.state == MapState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
if self.long_override:
|
||||
self.state = MapState.overriding
|
||||
else:
|
||||
self.state = MapState.enabled
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update(self, long_enabled: bool, long_override: bool, v_ego, a_ego, v_cruise) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.long_override = long_override
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
self.v_cruise = v_cruise
|
||||
|
||||
self.update_params()
|
||||
self.update_calculations()
|
||||
|
||||
self.is_enabled, self.is_active = self._update_state_machine()
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
@@ -1,19 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
|
||||
|
||||
class SmartCruiseControl:
|
||||
def __init__(self):
|
||||
self.vision = SmartCruiseControlVision()
|
||||
self.map = SmartCruiseControlMap()
|
||||
|
||||
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
|
||||
self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
|
||||
self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)
|
||||
@@ -1,58 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import platform
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
|
||||
|
||||
MapState = VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.MapState
|
||||
|
||||
|
||||
class TestSmartCruiseControlMap:
|
||||
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.reset_params()
|
||||
self.scc_m = SmartCruiseControlMap()
|
||||
|
||||
def reset_params(self):
|
||||
self.params.put_bool("SmartCruiseControlMap", True)
|
||||
|
||||
# TODO-SP: mock data from gpsLocation
|
||||
self.params.put("LastGPSPosition", "{}")
|
||||
self.params.put("MapTargetVelocities", "{}")
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
assert not self.scc_m.is_active
|
||||
assert self.scc_m.output_v_target == V_CRUISE_UNSET
|
||||
assert self.scc_m.output_a_target == 0.
|
||||
|
||||
def test_system_disabled(self):
|
||||
self.params.put_bool("SmartCruiseControlMap", False)
|
||||
self.scc_m.enabled = self.params.get_bool("SmartCruiseControlMap")
|
||||
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
assert not self.scc_m.is_active
|
||||
|
||||
def test_disabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(False, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.disabled
|
||||
|
||||
def test_transition_disabled_to_enabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_m.update(True, False, 0., 0., 0.)
|
||||
assert self.scc_m.state == VisionState.enabled
|
||||
|
||||
# TODO-SP: mock data from modelV2 to test other states
|
||||
@@ -1,104 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom, log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
|
||||
|
||||
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
|
||||
|
||||
|
||||
def generate_modelV2():
|
||||
model = messaging.new_message('modelV2')
|
||||
position = log.XYZTData.new_message()
|
||||
speed = 30
|
||||
position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
model.modelV2.position = position
|
||||
orientation = log.XYZTData.new_message()
|
||||
curvature = 0.05
|
||||
orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
|
||||
orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
|
||||
model.modelV2.orientation = orientation
|
||||
orientationRate = log.XYZTData.new_message()
|
||||
orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
|
||||
model.modelV2.orientationRate = orientationRate
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity.x[0] = float(speed) # always start at current speed
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
|
||||
return model
|
||||
|
||||
|
||||
def generate_carState():
|
||||
car_state = messaging.new_message('carState')
|
||||
speed = 30
|
||||
v_cruise = 50
|
||||
car_state.carState.vEgo = float(speed)
|
||||
car_state.carState.standstill = False
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
|
||||
return car_state
|
||||
|
||||
|
||||
def generate_controlsState():
|
||||
controls_state = messaging.new_message('controlsState')
|
||||
controls_state.controlsState.curvature = 0.05
|
||||
|
||||
return controls_state
|
||||
|
||||
|
||||
class TestSmartCruiseControlVision:
|
||||
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.reset_params()
|
||||
self.scc_v = SmartCruiseControlVision()
|
||||
|
||||
mdl = generate_modelV2()
|
||||
cs = generate_carState()
|
||||
controls_state = generate_controlsState()
|
||||
self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
|
||||
|
||||
def reset_params(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", True)
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
assert self.scc_v.output_v_target == V_CRUISE_UNSET
|
||||
assert self.scc_v.output_a_target == 0.
|
||||
|
||||
def test_system_disabled(self):
|
||||
self.params.put_bool("SmartCruiseControlVision", False)
|
||||
self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
assert not self.scc_v.is_active
|
||||
|
||||
def test_disabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, False, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.disabled
|
||||
|
||||
def test_transition_disabled_to_enabled(self):
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.scc_v.update(self.sm, True, False, 0., 0., 0.)
|
||||
assert self.scc_v.state == VisionState.enabled
|
||||
|
||||
# TODO-SP: mock modelV2 data to test other states
|
||||
@@ -1,203 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
|
||||
|
||||
VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
|
||||
|
||||
ACTIVE_STATES = (VisionState.entering, VisionState.turning, VisionState.leaving)
|
||||
ENABLED_STATES = (VisionState.enabled, VisionState.overriding, *ACTIVE_STATES)
|
||||
|
||||
_ENTERING_PRED_LAT_ACC_TH = 1.3 # Predicted Lat Acc threshold to trigger entering turn state.
|
||||
_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1 # Predicted Lat Acc threshold to abort entering state if speed drops.
|
||||
|
||||
_TURNING_LAT_ACC_TH = 1.6 # Lat Acc threshold to trigger turning state.
|
||||
|
||||
_LEAVING_LAT_ACC_TH = 1.3 # Lat Acc threshold to trigger leaving turn state.
|
||||
_FINISH_LAT_ACC_TH = 1.1 # Lat Acc threshold to trigger the end of the turn cycle.
|
||||
|
||||
_A_LAT_REG_MAX = 2. # Maximum lateral acceleration
|
||||
|
||||
_NO_OVERSHOOT_TIME_HORIZON = 4. # s. Time to use for velocity desired based on a_target when not overshooting.
|
||||
|
||||
# Lookup table for the minimum smooth deceleration during the ENTERING state
|
||||
# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
|
||||
_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.] # min decel value allowed on ENTERING state
|
||||
_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.] # absolute value of lat acc ahead
|
||||
|
||||
# Lookup table for the acceleration for the TURNING state
|
||||
# depending on the current lateral acceleration of the vehicle.
|
||||
_TURNING_ACC_V = [0.5, 0., -0.4] # acc value
|
||||
_TURNING_ACC_BP = [1.5, 2.3, 3.] # absolute value of current lat acc
|
||||
|
||||
_LEAVING_ACC = 0.5 # Conformable acceleration to regain speed while leaving a turn.
|
||||
|
||||
|
||||
class SmartCruiseControlVision:
|
||||
v_target: float = 0
|
||||
a_target: float = 0.
|
||||
v_ego: float = 0.
|
||||
a_ego: float = 0.
|
||||
output_v_target: float = V_CRUISE_UNSET
|
||||
output_a_target: float = 0.
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
self.long_enabled = False
|
||||
self.long_override = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
self.v_cruise_setpoint = 0.
|
||||
|
||||
self.state = VisionState.disabled
|
||||
self.current_lat_acc = 0.
|
||||
self.max_pred_lat_acc = 0.
|
||||
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_target
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self.is_active:
|
||||
return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
|
||||
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
def _update_params(self) -> None:
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.enabled = self.params.get_bool("SmartCruiseControlVision")
|
||||
|
||||
def _update_calculations(self, sm: messaging.SubMaster) -> None:
|
||||
if not self.long_enabled:
|
||||
return
|
||||
else:
|
||||
rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
|
||||
vel_plan = np.array(sm['modelV2'].velocity.x)
|
||||
|
||||
self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
|
||||
|
||||
# get the maximum lat accel from the model
|
||||
predicted_lat_accels = rate_plan * vel_plan
|
||||
self.max_pred_lat_acc = np.amax(predicted_lat_accels)
|
||||
|
||||
# get the maximum curve based on the current velocity
|
||||
v_ego = max(self.v_ego, 0.1) # ensure a value greater than 0 for calculations
|
||||
max_curve = self.max_pred_lat_acc / (v_ego**2)
|
||||
|
||||
# Get the target velocity for the maximum curve
|
||||
self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
|
||||
|
||||
def _update_state_machine(self) -> tuple[bool, bool]:
|
||||
# ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
|
||||
if self.state != VisionState.disabled:
|
||||
# longitudinal and feature disable always have priority in a non-disabled state
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = VisionState.disabled
|
||||
elif self.long_override:
|
||||
self.state = VisionState.overriding
|
||||
|
||||
else:
|
||||
# ENABLED
|
||||
if self.state == VisionState.enabled:
|
||||
# Do not enter a turn control cycle if the speed is low.
|
||||
if self.v_ego <= MIN_V:
|
||||
pass
|
||||
# If significant lateral acceleration is predicted ahead, then move to Entering turn state.
|
||||
elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.entering
|
||||
|
||||
# OVERRIDING
|
||||
elif self.state == VisionState.overriding:
|
||||
if not self.long_override:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# ENTERING
|
||||
elif self.state == VisionState.entering:
|
||||
# Transition to Turning if current lateral acceleration is over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Abort if the predicted lateral acceleration drops
|
||||
elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# Transition to Leaving if current lateral acceleration drops below a threshold.
|
||||
if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
|
||||
self.state = VisionState.leaving
|
||||
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# Transition back to Turning if current lateral acceleration goes back over the threshold.
|
||||
if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
|
||||
self.state = VisionState.turning
|
||||
# Finish if current lateral acceleration goes below a threshold.
|
||||
elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
# DISABLED
|
||||
elif self.state == VisionState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
if self.long_override:
|
||||
self.state = VisionState.overriding
|
||||
else:
|
||||
self.state = VisionState.enabled
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def _update_solution(self) -> float:
|
||||
# DISABLED, ENABLED, OVERRIDING
|
||||
if self.state not in ACTIVE_STATES:
|
||||
# when not overshooting, calculate v_turn as the speed at the prediction horizon when following
|
||||
# the smooth deceleration.
|
||||
a_target = self.a_ego
|
||||
# ENTERING
|
||||
elif self.state == VisionState.entering:
|
||||
# when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
|
||||
a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
|
||||
# TURNING
|
||||
elif self.state == VisionState.turning:
|
||||
# When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
|
||||
a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
|
||||
# LEAVING
|
||||
elif self.state == VisionState.leaving:
|
||||
# When leaving, we provide a comfortable acceleration to regain speed.
|
||||
a_target = _LEAVING_ACC
|
||||
else:
|
||||
raise NotImplementedError(f"SCC-V state not supported: {self.state}")
|
||||
|
||||
return a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
|
||||
v_cruise_setpoint: float) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.long_override = long_override
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
self.v_cruise_setpoint = v_cruise_setpoint
|
||||
|
||||
self._update_params()
|
||||
self._update_calculations(sm)
|
||||
|
||||
self.is_enabled, self.is_active = self._update_state_machine()
|
||||
self.a_target = self._update_solution()
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
@@ -1,19 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
|
||||
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
|
||||
|
||||
# Speed Limit Assist constants
|
||||
PCM_LONG_REQUIRED_MAX_SET_SPEED = {
|
||||
True: (33.3333, 36.1111), # km/h, (120, 130)
|
||||
False: (31.2928, 35.7632), # mph, (70, 80)
|
||||
}
|
||||
|
||||
CONFIRM_SPEED_THRESHOLD = {
|
||||
True: 80, # km/h
|
||||
False: 50, # mph
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from openpilot.sunnypilot import IntEnumBase
|
||||
|
||||
|
||||
class Policy(IntEnumBase):
|
||||
car_state_only = 0
|
||||
map_data_only = 1
|
||||
car_state_priority = 2
|
||||
map_data_priority = 3
|
||||
combined = 4
|
||||
|
||||
|
||||
class OffsetType(IntEnumBase):
|
||||
off = 0
|
||||
fixed = 1
|
||||
percentage = 2
|
||||
|
||||
|
||||
class Mode(IntEnumBase):
|
||||
off = 0
|
||||
information = 1
|
||||
warning = 2
|
||||
assist = 3
|
||||
@@ -1,44 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from cereal import custom, car
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
|
||||
|
||||
|
||||
def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
|
||||
speed_conv = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
|
||||
v_cruise_cluster_conv = round(v_cruise_cluster * speed_conv)
|
||||
target_set_speed_conv = round(target_set_speed * speed_conv)
|
||||
|
||||
req_plus = v_cruise_cluster_conv < target_set_speed_conv
|
||||
req_minus = v_cruise_cluster_conv > target_set_speed_conv
|
||||
|
||||
return req_plus, req_minus
|
||||
|
||||
|
||||
def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
|
||||
if params is None:
|
||||
params = Params()
|
||||
|
||||
is_release = params.get_bool("IsReleaseSpBranch")
|
||||
disallow_in_release = CP.brand == "tesla" and is_release
|
||||
always_disallow = CP.brand == "rivian"
|
||||
allowed = True
|
||||
|
||||
if disallow_in_release or always_disallow:
|
||||
allowed = False
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
|
||||
allowed = False
|
||||
|
||||
if not allowed:
|
||||
if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
|
||||
params.put("SpeedLimitMode", int(SpeedLimitMode.warning))
|
||||
|
||||
return allowed
|
||||
@@ -1,414 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import time
|
||||
|
||||
from cereal import custom, car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target, set_speed_limit_assist_availability
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
|
||||
ENABLED_STATES = (SpeedLimitAssistState.preActive, SpeedLimitAssistState.pending, *ACTIVE_STATES)
|
||||
|
||||
DISABLED_GUARD_PERIOD = 0.5 # secs.
|
||||
# secs. Time to wait after activation before considering temp deactivation signal.
|
||||
PRE_ACTIVE_GUARD_PERIOD = {
|
||||
True: 15,
|
||||
False: 5,
|
||||
}
|
||||
SPEED_LIMIT_CHANGED_HOLD_PERIOD = 1 # secs. Time to wait after speed limit change before switching to preActive.
|
||||
|
||||
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
|
||||
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
|
||||
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
|
||||
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
|
||||
V_CRUISE_UNSET = 255.
|
||||
|
||||
CRUISE_BUTTONS_PLUS = (ButtonType.accelCruise, ButtonType.resumeCruise)
|
||||
CRUISE_BUTTONS_MINUS = (ButtonType.decelCruise, ButtonType.setCruise)
|
||||
CRUISE_BUTTON_CONFIRM_HOLD = 0.5 # secs.
|
||||
|
||||
|
||||
class SpeedLimitAssist:
|
||||
_speed_limit_final_last: float
|
||||
_distance: float
|
||||
v_ego: float
|
||||
a_ego: float
|
||||
v_offset: float
|
||||
|
||||
def __init__(self, CP: car.CarParams, CP_SP: custom.CarParamsSP):
|
||||
self.params = Params()
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.frame = -1
|
||||
self.long_engaged_timer = 0
|
||||
self.pre_active_timer = 0
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
self.long_enabled = False
|
||||
self.long_enabled_prev = False
|
||||
self.is_enabled = False
|
||||
self.is_active = False
|
||||
self.output_v_target = V_CRUISE_UNSET
|
||||
self.output_a_target = 0.
|
||||
self.v_ego = 0.
|
||||
self.a_ego = 0.
|
||||
self.v_offset = 0.
|
||||
self.target_set_speed_conv = 0
|
||||
self.prev_target_set_speed_conv = 0
|
||||
self.v_cruise_cluster = 0.
|
||||
self.v_cruise_cluster_prev = 0.
|
||||
self.v_cruise_cluster_conv = 0
|
||||
self.prev_v_cruise_cluster_conv = 0
|
||||
self._has_speed_limit = False
|
||||
self._speed_limit = 0.
|
||||
self._speed_limit_final_last = 0.
|
||||
self.speed_limit_prev = 0.
|
||||
self.speed_limit_final_last_conv = 0
|
||||
self.prev_speed_limit_final_last_conv = 0
|
||||
self._distance = 0.
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
self._state_prev = SpeedLimitAssistState.disabled
|
||||
self.pcm_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
|
||||
|
||||
self._plus_hold = 0.
|
||||
self._minus_hold = 0.
|
||||
self._last_carstate_ts = 0.
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
# Solution functions mapped to respective states
|
||||
self.acceleration_solutions = {
|
||||
SpeedLimitAssistState.disabled: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.inactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.preActive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.pending: self.get_current_acceleration_as_target,
|
||||
SpeedLimitAssistState.adapting: self.get_adapting_state_target_acceleration,
|
||||
SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
|
||||
}
|
||||
|
||||
@property
|
||||
def speed_limit_changed(self) -> bool:
|
||||
return self._has_speed_limit and bool(self._speed_limit != self.speed_limit_prev)
|
||||
|
||||
@property
|
||||
def v_cruise_cluster_changed(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv != self.prev_v_cruise_cluster_conv)
|
||||
|
||||
@property
|
||||
def target_set_speed_confirmed(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv == self.target_set_speed_conv)
|
||||
|
||||
@property
|
||||
def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
|
||||
return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
|
||||
|
||||
def update_active_event(self, events_sp: EventsSP) -> None:
|
||||
if self.v_cruise_cluster_below_confirm_speed_threshold:
|
||||
events_sp.add(EventNameSP.speedLimitChanged)
|
||||
else:
|
||||
events_sp.add(EventNameSP.speedLimitActive)
|
||||
|
||||
def get_v_target_from_control(self) -> float:
|
||||
if self._has_speed_limit:
|
||||
if self.pcm_op_long and self.is_enabled:
|
||||
return self._speed_limit_final_last
|
||||
if not self.pcm_op_long and self.is_active:
|
||||
return self._speed_limit_final_last
|
||||
|
||||
# Fallback
|
||||
return V_CRUISE_UNSET
|
||||
|
||||
# TODO-SP: SLA's own output_a_target for planner
|
||||
def get_a_target_from_control(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
|
||||
self.enabled = self.params.get("SpeedLimitMode", return_default=True) == Mode.assist
|
||||
|
||||
def update_car_state(self, CS: car.CarState) -> None:
|
||||
now = time.monotonic()
|
||||
self._last_carstate_ts = now
|
||||
|
||||
for b in CS.buttonEvents:
|
||||
if not b.pressed:
|
||||
if b.type in CRUISE_BUTTONS_PLUS:
|
||||
self._plus_hold = max(self._plus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
elif b.type in CRUISE_BUTTONS_MINUS:
|
||||
self._minus_hold = max(self._minus_hold, now + CRUISE_BUTTON_CONFIRM_HOLD)
|
||||
|
||||
def _get_button_release(self, req_plus: bool, req_minus: bool) -> bool:
|
||||
now = time.monotonic()
|
||||
if req_plus and now <= self._plus_hold:
|
||||
self._plus_hold = 0.
|
||||
return True
|
||||
elif req_minus and now <= self._minus_hold:
|
||||
self._minus_hold = 0.
|
||||
return True
|
||||
|
||||
# expired
|
||||
if now > self._plus_hold:
|
||||
self._plus_hold = 0.
|
||||
if now > self._minus_hold:
|
||||
self._minus_hold = 0.
|
||||
return False
|
||||
|
||||
def update_calculations(self, v_cruise_cluster: float) -> None:
|
||||
speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH
|
||||
self.v_cruise_cluster = v_cruise_cluster
|
||||
|
||||
# Update current velocity offset (error)
|
||||
self.v_offset = self._speed_limit_final_last - self.v_ego
|
||||
|
||||
self.speed_limit_final_last_conv = round(self._speed_limit_final_last * speed_conv)
|
||||
self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
|
||||
|
||||
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric]
|
||||
pcm_long_required_max = cst_low if self._has_speed_limit and self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric] else \
|
||||
cst_high
|
||||
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
|
||||
|
||||
self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_last_conv
|
||||
|
||||
@property
|
||||
def apply_confirm_speed_threshold(self) -> bool:
|
||||
# below CST: always require user confirmation
|
||||
if self.v_cruise_cluster_below_confirm_speed_threshold:
|
||||
return True
|
||||
|
||||
# at/above CST:
|
||||
# - new speed limit >= CST: auto change
|
||||
# - new speed limit < CST: user confirmation required
|
||||
return bool(self.speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
|
||||
|
||||
def get_current_acceleration_as_target(self) -> float:
|
||||
return self.a_ego
|
||||
|
||||
def get_adapting_state_target_acceleration(self) -> float:
|
||||
if self._distance > 0:
|
||||
return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
|
||||
|
||||
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def get_active_state_target_acceleration(self) -> float:
|
||||
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def _update_confirmed_state(self):
|
||||
if self._has_speed_limit:
|
||||
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.adapting
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.active
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.pending
|
||||
|
||||
def _update_non_pcm_long_confirmed_state(self) -> bool:
|
||||
if self.target_set_speed_confirmed:
|
||||
return True
|
||||
|
||||
if self.state != SpeedLimitAssistState.preActive:
|
||||
return False
|
||||
|
||||
req_plus, req_minus = compare_cluster_target(self.v_cruise_cluster, self._speed_limit_final_last, self.is_metric)
|
||||
|
||||
return self._get_button_release(req_plus, req_minus)
|
||||
|
||||
def update_state_machine_pcm_op_long(self):
|
||||
self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
|
||||
# ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
|
||||
if self.state != SpeedLimitAssistState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
|
||||
else:
|
||||
# ACTIVE
|
||||
if self.state == SpeedLimitAssistState.active:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.adapting
|
||||
|
||||
# ADAPTING
|
||||
elif self.state == SpeedLimitAssistState.adapting:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self.v_offset >= LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitAssistState.active
|
||||
|
||||
# PENDING
|
||||
elif self.state == SpeedLimitAssistState.pending:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self.speed_limit_changed:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
|
||||
# PRE_ACTIVE
|
||||
elif self.state == SpeedLimitAssistState.preActive:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self.pre_active_timer <= 0:
|
||||
# Timeout - session ended
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
# INACTIVE
|
||||
elif self.state == SpeedLimitAssistState.inactive:
|
||||
pass
|
||||
|
||||
# DISABLED
|
||||
elif self.state == SpeedLimitAssistState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
# start or reset preActive timer if initially enabled or manual set speed change detected
|
||||
if not self.long_enabled_prev or self.v_cruise_cluster_changed:
|
||||
self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
|
||||
|
||||
elif self.long_engaged_timer <= 0:
|
||||
if self.target_set_speed_confirmed:
|
||||
self._update_confirmed_state()
|
||||
elif self._has_speed_limit:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.pending
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update_state_machine_non_pcm_long(self):
|
||||
self.long_engaged_timer = max(0, self.long_engaged_timer - 1)
|
||||
self.pre_active_timer = max(0, self.pre_active_timer - 1)
|
||||
|
||||
# ACTIVE, ADAPTING, PENDING, PRE_ACTIVE, INACTIVE
|
||||
if self.state != SpeedLimitAssistState.disabled:
|
||||
if not self.long_enabled or not self.enabled:
|
||||
self.state = SpeedLimitAssistState.disabled
|
||||
|
||||
else:
|
||||
# ACTIVE
|
||||
if self.state == SpeedLimitAssistState.active:
|
||||
if self.v_cruise_cluster_changed:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
elif self.speed_limit_changed and self.apply_confirm_speed_threshold:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
|
||||
# PRE_ACTIVE
|
||||
elif self.state == SpeedLimitAssistState.preActive:
|
||||
if self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
elif self.pre_active_timer <= 0:
|
||||
# Timeout - session ended
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
# INACTIVE
|
||||
elif self.state == SpeedLimitAssistState.inactive:
|
||||
if self.speed_limit_changed:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
elif self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
|
||||
# DISABLED
|
||||
elif self.state == SpeedLimitAssistState.disabled:
|
||||
if self.long_enabled and self.enabled:
|
||||
# start or reset preActive timer if initially enabled or manual set speed change detected
|
||||
if not self.long_enabled_prev or self.v_cruise_cluster_changed:
|
||||
self.long_engaged_timer = int(DISABLED_GUARD_PERIOD / DT_MDL)
|
||||
|
||||
elif self.long_engaged_timer <= 0:
|
||||
if self._update_non_pcm_long_confirmed_state():
|
||||
self.state = SpeedLimitAssistState.active
|
||||
elif self._has_speed_limit:
|
||||
self.state = SpeedLimitAssistState.preActive
|
||||
self.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.pcm_op_long] / DT_MDL)
|
||||
else:
|
||||
self.state = SpeedLimitAssistState.inactive
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
|
||||
def update_events(self, events_sp: EventsSP) -> None:
|
||||
if self.state == SpeedLimitAssistState.preActive:
|
||||
events_sp.add(EventNameSP.speedLimitPreActive)
|
||||
|
||||
if self.state == SpeedLimitAssistState.pending and self._state_prev != SpeedLimitAssistState.pending:
|
||||
events_sp.add(EventNameSP.speedLimitPending)
|
||||
|
||||
if self.is_active:
|
||||
if self._state_prev not in ACTIVE_STATES:
|
||||
self.update_active_event(events_sp)
|
||||
|
||||
# only notify if we acquire a valid speed limit
|
||||
# do not check has_speed_limit here
|
||||
elif self._speed_limit != self.speed_limit_prev:
|
||||
if self.speed_limit_prev <= 0:
|
||||
self.update_active_event(events_sp)
|
||||
elif self.speed_limit_prev > 0 and self._speed_limit > 0:
|
||||
self.update_active_event(events_sp)
|
||||
|
||||
def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
|
||||
speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
|
||||
self.long_enabled = long_enabled
|
||||
self.v_ego = v_ego
|
||||
self.a_ego = a_ego
|
||||
|
||||
self._has_speed_limit = has_speed_limit
|
||||
self._speed_limit = speed_limit
|
||||
self._speed_limit_final_last = speed_limit_final_last
|
||||
self._distance = distance
|
||||
|
||||
self.update_params()
|
||||
self.update_calculations(v_cruise_cluster)
|
||||
|
||||
self._state_prev = self.state
|
||||
if self.pcm_op_long:
|
||||
self.is_enabled, self.is_active = self.update_state_machine_pcm_op_long()
|
||||
else:
|
||||
self.is_enabled, self.is_active = self.update_state_machine_non_pcm_long()
|
||||
|
||||
self.update_events(events_sp)
|
||||
|
||||
# Update change tracking variables
|
||||
self.speed_limit_prev = self._speed_limit
|
||||
self.v_cruise_cluster_prev = self.v_cruise_cluster
|
||||
self.long_enabled_prev = self.long_enabled
|
||||
self.prev_target_set_speed_conv = self.target_set_speed_conv
|
||||
self.prev_v_cruise_cluster_conv = self.v_cruise_cluster_conv
|
||||
self.prev_speed_limit_final_last_conv = self.speed_limit_final_last_conv
|
||||
|
||||
self.output_v_target = self.get_v_target_from_control()
|
||||
self.output_a_target = self.get_a_target_from_control()
|
||||
|
||||
self.frame += 1
|
||||
@@ -1,190 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import time
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal import custom
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD, get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy, OffsetType
|
||||
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
ALL_SOURCES = tuple(SpeedLimitSource.schema.enumerants.values())
|
||||
|
||||
|
||||
class SpeedLimitResolver:
|
||||
limit_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
|
||||
distance_solutions: dict[custom.LongitudinalPlanSP.SpeedLimit.Source, float]
|
||||
v_ego: float
|
||||
speed_limit: float
|
||||
speed_limit_last: float
|
||||
speed_limit_final: float
|
||||
speed_limit_final_last: float
|
||||
distance: float
|
||||
source: custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
speed_limit_offset: float
|
||||
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
|
||||
self._gps_location_service = get_gps_location_service(self.params)
|
||||
self.limit_solutions = {} # Store for speed limit solutions from different sources
|
||||
self.distance_solutions = {} # Store for distance to current speed limit start for different sources
|
||||
|
||||
self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
|
||||
self.policy = get_sanitize_int_param(
|
||||
"SpeedLimitPolicy",
|
||||
Policy.min().value,
|
||||
Policy.max().value,
|
||||
self.params
|
||||
)
|
||||
self._policy_to_sources_map = {
|
||||
Policy.car_state_only: [SpeedLimitSource.car],
|
||||
Policy.map_data_only: [SpeedLimitSource.map],
|
||||
Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map],
|
||||
Policy.map_data_priority: [SpeedLimitSource.map, SpeedLimitSource.car],
|
||||
Policy.combined: [SpeedLimitSource.car, SpeedLimitSource.map],
|
||||
}
|
||||
self.source = SpeedLimitSource.none
|
||||
for source in ALL_SOURCES:
|
||||
self._reset_limit_sources(source)
|
||||
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.offset_type = get_sanitize_int_param(
|
||||
"SpeedLimitOffsetType",
|
||||
OffsetType.min().value,
|
||||
OffsetType.max().value,
|
||||
self.params
|
||||
)
|
||||
self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
|
||||
|
||||
self.speed_limit = 0.
|
||||
self.speed_limit_last = 0.
|
||||
self.speed_limit_final = 0.
|
||||
self.speed_limit_final_last = 0.
|
||||
self.speed_limit_offset = 0.
|
||||
|
||||
def update_speed_limit_states(self) -> None:
|
||||
self.speed_limit_final = self.speed_limit + self.speed_limit_offset
|
||||
|
||||
if self.speed_limit > 0.:
|
||||
self.speed_limit_last = self.speed_limit
|
||||
self.speed_limit_final_last = self.speed_limit_final
|
||||
|
||||
@property
|
||||
def speed_limit_valid(self) -> bool:
|
||||
return self.speed_limit > 0.
|
||||
|
||||
@property
|
||||
def speed_limit_last_valid(self) -> bool:
|
||||
return self.speed_limit_last > 0.
|
||||
|
||||
def update_params(self):
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
|
||||
self.policy = self.params.get("SpeedLimitPolicy", return_default=True)
|
||||
self.is_metric = self.params.get_bool("IsMetric")
|
||||
self.offset_type = self.params.get("SpeedLimitOffsetType", return_default=True)
|
||||
self.offset_value = self.params.get("SpeedLimitValueOffset", return_default=True)
|
||||
|
||||
def _get_speed_limit_offset(self) -> float:
|
||||
if self.offset_type == OffsetType.off:
|
||||
return 0
|
||||
elif self.offset_type == OffsetType.fixed:
|
||||
return float(self.offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS))
|
||||
elif self.offset_type == OffsetType.percentage:
|
||||
return float(self.offset_value * 0.01 * self.speed_limit)
|
||||
else:
|
||||
raise NotImplementedError("Offset not supported")
|
||||
|
||||
def _reset_limit_sources(self, source: custom.LongitudinalPlanSP.SpeedLimit.Source) -> None:
|
||||
self.limit_solutions[source] = 0.
|
||||
self.distance_solutions[source] = 0.
|
||||
|
||||
def _get_from_car_state(self, sm: messaging.SubMaster) -> None:
|
||||
self._reset_limit_sources(SpeedLimitSource.car)
|
||||
self.limit_solutions[SpeedLimitSource.car] = sm['carStateSP'].speedLimit
|
||||
self.distance_solutions[SpeedLimitSource.car] = 0.
|
||||
|
||||
def _get_from_map_data(self, sm: messaging.SubMaster) -> None:
|
||||
self._reset_limit_sources(SpeedLimitSource.map)
|
||||
self._process_map_data(sm)
|
||||
|
||||
def _process_map_data(self, sm: messaging.SubMaster) -> None:
|
||||
gps_data = sm[self._gps_location_service]
|
||||
map_data = sm['liveMapDataSP']
|
||||
|
||||
gps_fix_age = time.monotonic() - gps_data.unixTimestampMillis * 1e-3
|
||||
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
|
||||
return
|
||||
|
||||
speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
|
||||
next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
|
||||
|
||||
self._calculate_map_data_limits(sm, speed_limit, next_speed_limit)
|
||||
|
||||
def _calculate_map_data_limits(self, sm: messaging.SubMaster, speed_limit: float, next_speed_limit: float) -> None:
|
||||
gps_data = sm[self._gps_location_service]
|
||||
map_data = sm['liveMapDataSP']
|
||||
|
||||
distance_since_fix = self.v_ego * (time.monotonic() - gps_data.unixTimestampMillis * 1e-3)
|
||||
distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
|
||||
|
||||
self.limit_solutions[SpeedLimitSource.map] = speed_limit
|
||||
self.distance_solutions[SpeedLimitSource.map] = 0.
|
||||
|
||||
# FIXME-SP: this is not working as expected
|
||||
if 0. < next_speed_limit < self.v_ego:
|
||||
adapt_time = (next_speed_limit - self.v_ego) / LIMIT_ADAPT_ACC
|
||||
adapt_distance = self.v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
|
||||
|
||||
if distance_to_speed_limit_ahead <= adapt_distance:
|
||||
self.limit_solutions[SpeedLimitSource.map] = next_speed_limit
|
||||
self.distance_solutions[SpeedLimitSource.map] = distance_to_speed_limit_ahead
|
||||
|
||||
def _get_source_solution_according_to_policy(self) -> custom.LongitudinalPlanSP.SpeedLimit.Source:
|
||||
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
|
||||
return SpeedLimitSource.none
|
||||
|
||||
sources_with_limits = [(s, limit) for s, limit in [(s, self.limit_solutions[s]) for s in sources_for_policy] if limit > 0.]
|
||||
if sources_with_limits:
|
||||
return min(sources_with_limits, key=lambda x: x[1])[0]
|
||||
|
||||
return SpeedLimitSource.none
|
||||
|
||||
def _resolve_limit_sources(self, sm: messaging.SubMaster) -> tuple[float, float, custom.LongitudinalPlanSP.SpeedLimit.Source]:
|
||||
"""Get limit solutions from each data source"""
|
||||
self._get_from_car_state(sm)
|
||||
self._get_from_map_data(sm)
|
||||
|
||||
source = self._get_source_solution_according_to_policy()
|
||||
speed_limit = self.limit_solutions[source] if source else 0.
|
||||
distance = self.distance_solutions[source] if source else 0.
|
||||
|
||||
return speed_limit, distance, source
|
||||
|
||||
def update(self, v_ego: float, sm: messaging.SubMaster) -> None:
|
||||
self.v_ego = v_ego
|
||||
self.update_params()
|
||||
|
||||
self.speed_limit, self.distance, self.source = self._resolve_limit_sources(sm)
|
||||
self.speed_limit_offset = self._get_speed_limit_offset()
|
||||
|
||||
self.update_speed_limit_states()
|
||||
|
||||
self.frame += 1
|
||||
@@ -1,278 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from cereal import custom
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.rivian.values import CAR as RIVIAN
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist, \
|
||||
PRE_ACTIVE_GUARD_PERIOD, ACTIVE_STATES
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
|
||||
|
||||
ALL_STATES = tuple(SpeedLimitAssistState.schema.enumerants.values())
|
||||
|
||||
SPEED_LIMITS = {
|
||||
'residential': 25 * CV.MPH_TO_MS, # 25 mph
|
||||
'city': 35 * CV.MPH_TO_MS, # 35 mph
|
||||
'highway': 65 * CV.MPH_TO_MS, # 65 mph
|
||||
'freeway': 80 * CV.MPH_TO_MS, # 80 mph
|
||||
}
|
||||
|
||||
DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def car_name(request):
|
||||
return getattr(request, "param", DEFAULT_CAR)
|
||||
|
||||
|
||||
@pytest.fixture(autouse=True)
|
||||
def set_car_name_on_instance(request, car_name):
|
||||
instance = getattr(request, "instance", None)
|
||||
if instance:
|
||||
instance.car_name = car_name
|
||||
|
||||
|
||||
class TestSpeedLimitAssist:
|
||||
|
||||
def setup_method(self, method):
|
||||
self.params = Params()
|
||||
self.reset_custom_params()
|
||||
self.events_sp = EventsSP()
|
||||
CI = self._setup_platform(self.car_name)
|
||||
self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
|
||||
self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)
|
||||
self.pcm_long_max_set_speed = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric][1] # use 80 MPH for now
|
||||
self.speed_conv = CV.MS_TO_KPH if self.sla.is_metric else CV.MS_TO_MPH
|
||||
|
||||
def teardown_method(self, method):
|
||||
self.reset_state()
|
||||
|
||||
def _setup_platform(self, car_name):
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
CI.CP.openpilotLongitudinalControl = True # always assume it's openpilot longitudinal
|
||||
sunnypilot_interfaces.setup_interfaces(CI, self.params)
|
||||
return CI
|
||||
|
||||
def reset_custom_params(self):
|
||||
self.params.put("IsReleaseSpBranch", True)
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist))
|
||||
self.params.put_bool("IsMetric", False)
|
||||
self.params.put("SpeedLimitOffsetType", 0)
|
||||
self.params.put("SpeedLimitValueOffset", 0)
|
||||
|
||||
def reset_state(self):
|
||||
self.sla.state = SpeedLimitAssistState.disabled
|
||||
self.sla.frame = -1
|
||||
self.sla.last_op_engaged_frame = 0
|
||||
self.sla.op_engaged = False
|
||||
self.sla.op_engaged_prev = False
|
||||
self.sla._speed_limit = 0.
|
||||
self.sla.speed_limit_prev = 0.
|
||||
self.sla.last_valid_speed_limit_offsetted = 0.
|
||||
self.sla._distance = 0.
|
||||
self.events_sp.clear()
|
||||
|
||||
def initialize_active_state(self, initialize_v_cruise):
|
||||
self.sla.state = SpeedLimitAssistState.active
|
||||
self.sla.v_cruise_cluster = initialize_v_cruise
|
||||
self.sla.v_cruise_cluster_prev = initialize_v_cruise
|
||||
self.sla.prev_v_cruise_cluster_conv = round(initialize_v_cruise * self.speed_conv)
|
||||
|
||||
def test_initial_state(self):
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
assert not self.sla.is_enabled
|
||||
assert not self.sla.is_active
|
||||
assert V_CRUISE_UNSET == self.sla.get_v_target_from_control()
|
||||
|
||||
@pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1_GEN1, TESLA.TESLA_MODEL_Y], indirect=True)
|
||||
def test_disallowed_brands(self, car_name):
|
||||
"""
|
||||
Speed Limit Assist is disabled for the following brands and conditions:
|
||||
- All Tesla and is a release branch;
|
||||
- All Rivian
|
||||
"""
|
||||
assert not self.sla.enabled
|
||||
|
||||
# stay disallowed even when the param may have changed from somewhere else
|
||||
self.params.put("SpeedLimitMode", int(Mode.assist))
|
||||
for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'],
|
||||
SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert not self.sla.enabled
|
||||
|
||||
def test_disabled(self):
|
||||
self.params.put("SpeedLimitMode", int(Mode.off))
|
||||
for _ in range(int(10. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
|
||||
def test_transition_disabled_to_preactive(self):
|
||||
for _ in range(int(3. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.preActive
|
||||
assert self.sla.is_enabled and not self.sla.is_active
|
||||
|
||||
def test_transition_disabled_to_pending_no_speed_limit_not_max_initial_set_speed(self):
|
||||
for _ in range(int(3. / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, SPEED_LIMITS['city'], 0, 0, False, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.pending
|
||||
assert self.sla.is_enabled and not self.sla.is_active
|
||||
|
||||
def test_preactive_to_active_with_max_speed_confirmation(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.active
|
||||
assert self.sla.is_enabled and self.sla.is_active
|
||||
assert self.sla.output_v_target == SPEED_LIMITS['highway']
|
||||
|
||||
def test_preactive_timeout_to_inactive(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
|
||||
for _ in range(int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / DT_MDL)):
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.inactive
|
||||
|
||||
def test_preactive_to_pending_no_speed_limit(self):
|
||||
self.sla.state = SpeedLimitAssistState.preActive
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed, 0, 0, False, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.pending
|
||||
assert self.sla.is_enabled and not self.sla.is_active
|
||||
|
||||
def test_pending_to_active_when_speed_limit_available(self):
|
||||
self.sla.state = SpeedLimitAssistState.pending
|
||||
self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'], 0, self.pcm_long_max_set_speed,
|
||||
SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.active
|
||||
|
||||
def test_pending_to_adapting_when_below_speed_limit(self):
|
||||
self.sla.state = SpeedLimitAssistState.pending
|
||||
self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'] + 5, 0, self.pcm_long_max_set_speed,
|
||||
SPEED_LIMITS['highway'], SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.adapting
|
||||
assert self.sla.is_enabled and self.sla.is_active
|
||||
|
||||
def test_active_to_adapting_transition(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['highway'] + 2, 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.adapting
|
||||
|
||||
def test_adapting_to_active_transition(self):
|
||||
self.sla.state = SpeedLimitAssistState.adapting
|
||||
self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['highway'],
|
||||
SPEED_LIMITS['highway'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.active
|
||||
|
||||
def test_manual_cruise_change_detection(self):
|
||||
self.sla.state = SpeedLimitAssistState.active
|
||||
expected_cruise = SPEED_LIMITS['highway']
|
||||
self.sla.v_cruise_cluster_prev = expected_cruise
|
||||
|
||||
different_cruise = SPEED_LIMITS['highway'] + 5
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.inactive
|
||||
|
||||
# TODO-SP: test lower CST cases
|
||||
def test_rapid_speed_limit_changes(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
speed_limits = [SPEED_LIMITS['highway'], SPEED_LIMITS['freeway']]
|
||||
|
||||
for _, speed_limit in enumerate(speed_limits):
|
||||
self.sla.update(True, False, speed_limit, 0, self.pcm_long_max_set_speed, speed_limit, speed_limit, True, 0, self.events_sp)
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
|
||||
def test_invalid_speed_limits_handling(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
|
||||
invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
|
||||
|
||||
for invalid_limit in invalid_limits:
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert isinstance(self.sla.output_v_target, (int, float))
|
||||
assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
|
||||
|
||||
def test_stale_data_handling(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
old_speed_limit = SPEED_LIMITS['city']
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, 0, old_speed_limit, True, 0, self.events_sp)
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
assert self.sla.output_v_target == old_speed_limit
|
||||
|
||||
def test_distance_based_adapting(self):
|
||||
self.sla.state = SpeedLimitAssistState.adapting
|
||||
self.sla.v_cruise_cluster_prev = self.pcm_long_max_set_speed
|
||||
self.sla.prev_v_cruise_cluster_conv = round(self.pcm_long_max_set_speed * self.speed_conv)
|
||||
|
||||
distance = 100.0
|
||||
current_speed = SPEED_LIMITS['freeway']
|
||||
target_speed = SPEED_LIMITS['highway']
|
||||
|
||||
self.sla.update(True, False, current_speed, 0, self.pcm_long_max_set_speed, target_speed, target_speed, True, distance, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.adapting
|
||||
assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
|
||||
|
||||
def test_long_disengaged_to_disabled(self):
|
||||
self.initialize_active_state(self.pcm_long_max_set_speed)
|
||||
|
||||
self.sla.update(False, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'],
|
||||
SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
assert self.sla.state == SpeedLimitAssistState.disabled
|
||||
assert self.sla.output_v_target == V_CRUISE_UNSET
|
||||
|
||||
def test_maintain_states_with_no_changes(self):
|
||||
"""Test that states are maintained when no significant changes occur"""
|
||||
test_states = [
|
||||
SpeedLimitAssistState.preActive,
|
||||
SpeedLimitAssistState.pending,
|
||||
SpeedLimitAssistState.active,
|
||||
SpeedLimitAssistState.adapting
|
||||
]
|
||||
|
||||
for state in test_states:
|
||||
self.sla.state = state
|
||||
self.sla.op_engaged = True
|
||||
|
||||
initial_state = state
|
||||
|
||||
self.sla.update(True, False, SPEED_LIMITS['city'], 0, self.pcm_long_max_set_speed, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
|
||||
|
||||
assert self.sla.state in ALL_STATES # Sanity check
|
||||
|
||||
if initial_state == SpeedLimitAssistState.preActive:
|
||||
assert self.sla.state in [SpeedLimitAssistState.preActive, SpeedLimitAssistState.active]
|
||||
elif initial_state in ACTIVE_STATES:
|
||||
assert self.sla.state in ACTIVE_STATES
|
||||
@@ -1,144 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import random
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from pytest_mock import MockerFixture
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
|
||||
|
||||
SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
|
||||
|
||||
|
||||
def create_mock(properties, mocker: MockerFixture):
|
||||
mock = mocker.MagicMock()
|
||||
for _property, value in properties.items():
|
||||
setattr(mock, _property, value)
|
||||
return mock
|
||||
|
||||
|
||||
def setup_sm_mock(mocker: MockerFixture):
|
||||
cruise_speed_limit = random.uniform(0, 120)
|
||||
live_map_data_limit = random.uniform(0, 120)
|
||||
|
||||
car_state = create_mock({
|
||||
'gasPressed': False,
|
||||
'brakePressed': False,
|
||||
'standstill': False,
|
||||
}, mocker)
|
||||
car_state_sp = create_mock({
|
||||
'speedLimit': cruise_speed_limit,
|
||||
}, mocker)
|
||||
live_map_data = create_mock({
|
||||
'speedLimit': live_map_data_limit,
|
||||
'speedLimitValid': True,
|
||||
'speedLimitAhead': 0.,
|
||||
'speedLimitAheadValid': 0.,
|
||||
'speedLimitAheadDistance': 0.,
|
||||
}, mocker)
|
||||
gps_data = create_mock({
|
||||
'unixTimestampMillis': time.monotonic() * 1e3,
|
||||
}, mocker)
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock.__getitem__.side_effect = lambda key: {
|
||||
'carState': car_state,
|
||||
'liveMapDataSP': live_map_data,
|
||||
'carStateSP': car_state_sp,
|
||||
'gpsLocation': gps_data,
|
||||
}[key]
|
||||
return sm_mock
|
||||
|
||||
|
||||
parametrized_policies = pytest.mark.parametrize(
|
||||
"policy, sm_key, function_key", [
|
||||
(Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
|
||||
(Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
|
||||
(Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
|
||||
(Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
|
||||
],
|
||||
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
|
||||
class TestSpeedLimitResolverValidation:
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_initial_state(self, resolver_class, policy):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
for source in ALL_SOURCES:
|
||||
if source in resolver.limit_solutions:
|
||||
assert resolver.limit_solutions[source] == 0.
|
||||
assert resolver.distance_solutions[source] == 0.
|
||||
|
||||
@parametrized_policies
|
||||
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the resolver
|
||||
resolver.update(source_speed_limit, sm_mock)
|
||||
assert resolver.speed_limit == source_speed_limit
|
||||
assert resolver.source == ALL_SOURCES[function_key]
|
||||
|
||||
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = Policy.combined
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
|
||||
minimum_key, minimum_speed_limit = min(
|
||||
((key, sm_mock[key].speedLimit) for key in
|
||||
socket_to_source.keys()), key=lambda x: x[1])
|
||||
|
||||
# Assert the resolver
|
||||
resolver.update(minimum_speed_limit, sm_mock)
|
||||
assert resolver.speed_limit == minimum_speed_limit
|
||||
assert resolver.source == socket_to_source[minimum_key]
|
||||
|
||||
@parametrized_policies
|
||||
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the parsing
|
||||
resolver.update(source_speed_limit, sm_mock)
|
||||
assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
|
||||
assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
|
||||
v_ego = 50
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
resolver.update(v_ego, sm_mock)
|
||||
|
||||
# After resolution
|
||||
assert resolver.speed_limit is not None
|
||||
assert resolver.distance is not None
|
||||
assert resolver.source is not None
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
|
||||
resolver = resolver_class()
|
||||
resolver.policy = policy
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
|
||||
resolver._get_from_map_data(sm_mock)
|
||||
assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
|
||||
assert resolver.distance_solutions[SpeedLimitSource.map] == 0.
|
||||
@@ -1,9 +1,7 @@
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log, car, custom
|
||||
from openpilot.common.constants import CV
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
|
||||
NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
|
||||
|
||||
|
||||
AlertSize = log.SelfdriveState.AlertSize
|
||||
@@ -18,43 +16,6 @@ EventNameSP = custom.OnroadEventSP.EventName
|
||||
EVENT_NAME_SP = {v: k for k, v in EventNameSP.schema.enumerants.items()}
|
||||
|
||||
|
||||
def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
speedLimit = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimit
|
||||
speed = round(speedLimit * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))
|
||||
message = f'Adjusting to {speed} {"km/h" if metric else "mph"} speed limit'
|
||||
return Alert(
|
||||
message,
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
|
||||
|
||||
|
||||
def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
|
||||
speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
|
||||
speed_limit_final_last = sm['longitudinalPlanSP'].speedLimit.resolver.speedLimitFinalLast
|
||||
speed_limit_final_last_conv = round(speed_limit_final_last * speed_conv)
|
||||
alert_1_str = ""
|
||||
alert_2_str = ""
|
||||
alert_size = AlertSize.none
|
||||
|
||||
if CP.openpilotLongitudinalControl and CP.pcmCruise:
|
||||
# PCM long
|
||||
cst_low, cst_high = PCM_LONG_REQUIRED_MAX_SET_SPEED[metric]
|
||||
pcm_long_required_max = cst_low if speed_limit_final_last_conv < CONFIRM_SPEED_THRESHOLD[metric] else cst_high
|
||||
pcm_long_required_max_set_speed_conv = round(pcm_long_required_max * speed_conv)
|
||||
speed_unit = "km/h" if metric else "mph"
|
||||
|
||||
alert_1_str = "Speed Limit Assist: Activation Required"
|
||||
alert_2_str = f"Manually change set speed to {pcm_long_required_max_set_speed_conv} {speed_unit} to activate"
|
||||
alert_size = AlertSize.mid
|
||||
|
||||
return Alert(
|
||||
alert_1_str,
|
||||
alert_2_str,
|
||||
AlertStatus.normal, alert_size,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
|
||||
|
||||
|
||||
class EventsSP(EventsBase):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
@@ -191,34 +152,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitActive: {
|
||||
ET.WARNING: Alert(
|
||||
"Automatically adjusting to the posted speed limit",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitChanged: {
|
||||
ET.WARNING: Alert(
|
||||
"Set speed changed",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitPreActive: {
|
||||
ET.WARNING: speed_limit_pre_active_alert,
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitPending: {
|
||||
ET.WARNING: Alert(
|
||||
"Automatically adjusting to the last speed limit",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleHigh, 5.),
|
||||
},
|
||||
|
||||
EventNameSP.e2eChime: {
|
||||
ET.PERMANENT: Alert(
|
||||
"",
|
||||
|
||||
2
third_party/mapd_pfeiferj/README.md
vendored
2
third_party/mapd_pfeiferj/README.md
vendored
@@ -1,2 +0,0 @@
|
||||
# MAPD implementation by pfeiferj
|
||||
https://github.com/pfeiferj/openpilot-mapd/releases/
|
||||
BIN
third_party/mapd_pfeiferj/mapd
vendored
BIN
third_party/mapd_pfeiferj/mapd
vendored
Binary file not shown.
Reference in New Issue
Block a user