mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-04 03:53:45 +08:00
Merge branch 'mapd' into feature/slc
This commit is contained in:
Jason Wen
@@ -102,9 +102,6 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
|
||||
struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
dec @0 :DynamicExperimentalControl;
|
||||
|
||||
events @1 :List(OnroadEventSP.Event);
|
||||
slc @2 :SpeedLimitControl;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
enabled @1 :Bool;
|
||||
@@ -115,23 +112,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
struct SpeedLimitControl {
|
||||
state @0 :SpeedLimitControlState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
speedLimit @3 :Float32;
|
||||
speedLimitOffset @4 :Float32;
|
||||
distToSpeedLimit @5 :Float32;
|
||||
}
|
||||
|
||||
enum SpeedLimitControlState {
|
||||
inactive @0; # No speed limit set or not enabled by parameter.
|
||||
tempInactive @1; # User wants to ignore speed limit until it changes.
|
||||
preActive @2;
|
||||
adapting @3; # Reducing speed to match new speed limit.
|
||||
active @4; # Cruising at speed limit.
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
@@ -171,10 +151,6 @@ struct OnroadEventSP @0xda96579883444c35 {
|
||||
experimentalModeSwitched @14;
|
||||
wrongCarModeAlertOnly @15;
|
||||
pedalPressedAlertOnly @16;
|
||||
speedLimitPreActive @17;
|
||||
speedLimitActive @18;
|
||||
speedLimitConfirmed @19;
|
||||
speedLimitValueChange @20;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -248,7 +224,6 @@ struct BackupManagerSP @0xf98d843bfd7004a3 {
|
||||
}
|
||||
|
||||
struct CarStateSP @0xb86e6369214c01c8 {
|
||||
speedLimit @0 :Float32; # m/s
|
||||
}
|
||||
|
||||
struct LiveMapDataSP @0xf416ec09499d9d19 {
|
||||
@@ -272,13 +247,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
|
||||
lastGpsBearingDeg @17 :Float32;
|
||||
lastGpsAccuracy @18 :Float32;
|
||||
lastGpsBearingAccuracyDeg @19 :Float32;
|
||||
dataType @20 :DataType;
|
||||
|
||||
enum DataType {
|
||||
default @0;
|
||||
offline @1;
|
||||
online @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct CustomReserved9 @0xa1680744031fdb2d {
|
||||
|
||||
+1
-1
@@ -84,7 +84,7 @@ _services: dict[str, tuple] = {
|
||||
"carParamsSP": (True, 0.02, 1),
|
||||
"carControlSP": (True, 100., 10),
|
||||
"carStateSP": (True, 100., 10),
|
||||
"liveMapDataSP": (True, 1.),
|
||||
"liveMapDataSP": (True, 1., 1),
|
||||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
|
||||
+10
-20
@@ -177,32 +177,22 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
||||
{"LagdToggle", PERSISTENT | BACKUP},
|
||||
|
||||
// mapd
|
||||
{"MapdVersion", PERSISTENT},
|
||||
{"RoadName", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"MapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"MapAdvisorySpeedLimit", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"MapdVersion", PERSISTENT},
|
||||
{"MapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"NextMapSpeedLimit", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"OSMDownloadBounds", PERSISTENT},
|
||||
{"OSMDownloadLocations", PERSISTENT},
|
||||
{"OsmDownloadedDate", PERSISTENT},
|
||||
{"OsmStateTitle", PERSISTENT},
|
||||
{"OsmStateName", PERSISTENT},
|
||||
{"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
|
||||
{"Offroad_OSMUpdateRequired", CLEAR_ON_MANAGER_START},
|
||||
{"OsmDbUpdatesCheck", CLEAR_ON_MANAGER_START}, // mapd database update happens with device ON, reset on boot
|
||||
{"OSMDownloadBounds", PERSISTENT},
|
||||
{"OsmDownloadedDate", PERSISTENT},
|
||||
{"OSMDownloadLocations", PERSISTENT},
|
||||
{"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
|
||||
{"OsmLocal", PERSISTENT},
|
||||
{"OsmLocationName", PERSISTENT},
|
||||
{"OsmLocationTitle", PERSISTENT},
|
||||
{"OsmLocationUrl", PERSISTENT},
|
||||
{"OsmStateName", PERSISTENT},
|
||||
{"OsmStateTitle", PERSISTENT},
|
||||
{"OsmWayTest", PERSISTENT},
|
||||
{"Offroad_OSMUpdateRequired", CLEAR_ON_MANAGER_START},
|
||||
|
||||
// Speed Limit Control
|
||||
{"SpeedLimitControl", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitControlPolicy", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitEngageType", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitOffsetType", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitValueOffset", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitWarningType", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitWarningOffsetType", PERSISTENT | BACKUP},
|
||||
{"SpeedLimitWarningValueOffset", PERSISTENT | BACKUP},
|
||||
{"RoadName", CLEAR_ON_ONROAD_TRANSITION},
|
||||
};
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: bd447a9cec...202a26d943
@@ -144,9 +144,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
|
||||
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
|
||||
|
||||
# Get new v_cruise from Speed Limit Control
|
||||
v_cruise = LongitudinalPlannerSP.update_v_cruise(self, sm, not reset_state, self.v_desired_filter.x, self.a_desired, v_cruise)
|
||||
|
||||
if force_slow_decel:
|
||||
v_cruise = 0.0
|
||||
|
||||
|
||||
@@ -19,8 +19,7 @@ def main():
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||
"liveMapDataSP", "carStateSP"],
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
|
||||
poll='modelV2')
|
||||
|
||||
while True:
|
||||
|
||||
@@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper):
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
|
||||
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
|
||||
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userFlag'] + \
|
||||
self.camera_packets + self.sensor_packets + self.gps_packets + ["longitudinalPlanSP"],
|
||||
self.camera_packets + self.sensor_packets + self.gps_packets,
|
||||
ignore_alive=ignore, ignore_avg_freq=ignore,
|
||||
ignore_valid=ignore, frequency=int(1/DT_CTRL))
|
||||
|
||||
@@ -196,7 +196,6 @@ class SelfdriveD(CruiseHelper):
|
||||
|
||||
if not self.CP.notCar:
|
||||
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
|
||||
self.events_sp.add_from_msg(self.sm['longitudinalPlanSP'].events)
|
||||
|
||||
# Add car events, ignore if CAN isn't valid
|
||||
if CS.canValid:
|
||||
|
||||
@@ -1,11 +1,9 @@
|
||||
from cereal import custom
|
||||
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
|
||||
DataType = custom.LiveMapDataSP.DataType
|
||||
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.
|
||||
|
||||
@@ -1,11 +1,10 @@
|
||||
import time
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
from cereal import custom, messaging
|
||||
from cereal import messaging
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate
|
||||
from openpilot.sunnypilot.mapd.live_map_data import get_debug
|
||||
|
||||
|
||||
class BaseMapData(ABC):
|
||||
@@ -14,12 +13,11 @@ class BaseMapData(ABC):
|
||||
self._last_gps: Coordinate | None = None
|
||||
self._gps_location_service = get_gps_location_service(self.params)
|
||||
self._gps_packets = [self._gps_location_service]
|
||||
self._data_type = custom.LiveMapDataSP.DataType.default
|
||||
self._sm = messaging.SubMaster(['livePose', 'carControl'] + self._gps_packets)
|
||||
self._pm = messaging.PubMaster(['liveMapDataSP'])
|
||||
|
||||
@abstractmethod
|
||||
def update_location(self, current_location: Coordinate):
|
||||
def update_location(self, current_location: Coordinate | None) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
@@ -27,7 +25,7 @@ class BaseMapData(ABC):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
|
||||
def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
@@ -59,49 +57,34 @@ class BaseMapData(ABC):
|
||||
|
||||
return result
|
||||
|
||||
def get_live_map_data_sp(self, speed_limit, next_speed_limit, next_speed_limit_distance, current_road_name):
|
||||
last_gps = self.get_current_location()
|
||||
map_data_msg = messaging.new_message('liveMapDataSP')
|
||||
map_data_msg.valid = self._is_gps_data_valid()
|
||||
def publish(self) -> None:
|
||||
speed_limit = self.get_current_speed_limit()
|
||||
current_road_name = self.get_current_road_name()
|
||||
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance(self._last_gps)
|
||||
|
||||
live_map_data = map_data_msg.liveMapDataSP
|
||||
if last_gps:
|
||||
live_map_data.lastGpsTimestamp = last_gps.annotations.get('unixTimestampMillis', 0)
|
||||
live_map_data.lastGpsLatitude = last_gps.latitude
|
||||
live_map_data.lastGpsLongitude = last_gps.longitude
|
||||
live_map_data.lastGpsSpeed = last_gps.annotations.get('speed', 0)
|
||||
live_map_data.lastGpsBearingDeg = last_gps.annotations.get('bearingDeg', 0)
|
||||
live_map_data.lastGpsAccuracy = last_gps.annotations.get('accuracy', 0)
|
||||
live_map_data.lastGpsBearingAccuracyDeg = last_gps.annotations.get('bearingAccuracyDeg', 0)
|
||||
mapd_sp_send = messaging.new_message('liveMapDataSP')
|
||||
mapd_sp_send.valid = self._is_gps_data_valid()
|
||||
live_map_data = mapd_sp_send.liveMapDataSP
|
||||
|
||||
if self._last_gps:
|
||||
live_map_data.lastGpsTimestamp = self._last_gps.annotations.get('unixTimestampMillis', 0)
|
||||
live_map_data.lastGpsLatitude = self._last_gps.latitude
|
||||
live_map_data.lastGpsLongitude = self._last_gps.longitude
|
||||
live_map_data.lastGpsSpeed = self._last_gps.annotations.get('speed', 0)
|
||||
live_map_data.lastGpsBearingDeg = self._last_gps.annotations.get('bearingDeg', 0)
|
||||
live_map_data.lastGpsAccuracy = self._last_gps.annotations.get('accuracy', 0)
|
||||
live_map_data.lastGpsBearingAccuracyDeg = self._last_gps.annotations.get('bearingAccuracyDeg', 0)
|
||||
|
||||
live_map_data.speedLimitValid = bool(speed_limit > 0)
|
||||
live_map_data.speedLimit = speed_limit
|
||||
live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0)
|
||||
live_map_data.speedLimitAhead = float(next_speed_limit)
|
||||
live_map_data.speedLimitAheadDistance = float(next_speed_limit_distance)
|
||||
live_map_data.currentRoadName = str(current_road_name)
|
||||
live_map_data.dataType = self._data_type
|
||||
live_map_data.speedLimitAhead = next_speed_limit
|
||||
live_map_data.speedLimitAheadDistance = next_speed_limit_distance
|
||||
live_map_data.currentRoadName = current_road_name
|
||||
|
||||
return map_data_msg
|
||||
self._pm.send('liveMapDataSP', mapd_sp_send)
|
||||
|
||||
def publish(self):
|
||||
speed_limit = self.get_current_speed_limit()
|
||||
current_road_name = self.get_current_road_name()
|
||||
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
|
||||
|
||||
live_map_data_sp = self.get_live_map_data_sp(
|
||||
speed_limit,
|
||||
next_speed_limit,
|
||||
next_speed_limit_distance,
|
||||
current_road_name
|
||||
)
|
||||
|
||||
self._pm.send('liveMapDataSP', live_map_data_sp)
|
||||
get_debug(f"SRC: [{self.__class__.__name__}] | SLC: [{speed_limit}] | NSL: [{next_speed_limit}] | " +
|
||||
f"NSLD: [{next_speed_limit_distance}] | CRN: [{current_road_name}] | GPS: [{self._last_gps}] " +
|
||||
f"Annotations: [{', '.join(f'{key}: {value}' for key, value in self._last_gps.annotations.items()) if self._last_gps else []}]")
|
||||
|
||||
def tick(self):
|
||||
def tick(self) -> None:
|
||||
self._sm.update()
|
||||
self._last_gps = self.get_current_location()
|
||||
self.update_location(self._last_gps)
|
||||
|
||||
@@ -9,11 +9,10 @@ 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 set_core_affinity
|
||||
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
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
def excepthook(args):
|
||||
@@ -22,10 +21,7 @@ def excepthook(args):
|
||||
|
||||
|
||||
def live_map_data_sp_thread():
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("mapd: failed to set core affinity")
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
params = Params()
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
|
||||
@@ -3,39 +3,33 @@ import platform
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate
|
||||
from openpilot.sunnypilot.mapd.live_map_data import DataType
|
||||
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
|
||||
|
||||
|
||||
class OsmMapData(BaseMapData):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.last_gps = Coordinate(0.0, 0.0)
|
||||
self.params = Params()
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.data_type = DataType.offline
|
||||
|
||||
def update_location(self, current_location):
|
||||
self.last_gps = current_location
|
||||
if not self.last_gps:
|
||||
def update_location(self, current_location: Coordinate | None) -> None:
|
||||
if not current_location:
|
||||
return
|
||||
|
||||
last_gps_position_for_osm = {
|
||||
"latitude": self.last_gps.latitude,
|
||||
"longitude": self.last_gps.longitude,
|
||||
"bearing": self.last_gps.annotations.get("bearingDeg", 0)
|
||||
"latitude": current_location.latitude,
|
||||
"longitude": current_location.longitude,
|
||||
"bearing": current_location.annotations.get("bearingDeg", 0)
|
||||
}
|
||||
self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm))
|
||||
|
||||
def get_current_speed_limit(self):
|
||||
speed_limit = self.mem_params.get("MapSpeedLimit", encoding='utf8')
|
||||
return float(speed_limit) if speed_limit else 0.0
|
||||
def get_current_speed_limit(self) -> float:
|
||||
return float(self.mem_params.get("MapSpeedLimit", encoding='utf8') or 0.0)
|
||||
|
||||
def get_current_road_name(self):
|
||||
current_road_name = self.mem_params.get("RoadName", encoding='utf8')
|
||||
return current_road_name if current_road_name else ""
|
||||
def get_current_road_name(self) -> str:
|
||||
return self.mem_params.get("RoadName", encoding='utf8') or ""
|
||||
|
||||
def get_next_speed_limit_and_distance(self) -> tuple[float, float]:
|
||||
def get_next_speed_limit_and_distance(self, current_location: Coordinate | None) -> tuple[float, float]:
|
||||
next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8')
|
||||
next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {}
|
||||
next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
|
||||
@@ -45,6 +39,6 @@ class OsmMapData(BaseMapData):
|
||||
|
||||
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_gps or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
|
||||
next_speed_limit_distance = (current_location or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
|
||||
|
||||
return next_speed_limit, next_speed_limit_distance
|
||||
|
||||
@@ -6,10 +6,9 @@
|
||||
import threading
|
||||
import traceback
|
||||
|
||||
from openpilot.common.realtime import Ratekeeper, set_core_affinity
|
||||
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
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
def excepthook(args):
|
||||
@@ -18,10 +17,7 @@ def excepthook(args):
|
||||
|
||||
|
||||
def live_map_data_sp_thread():
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("mapd: failed to set core affinity")
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
live_map_sp = OsmMapData()
|
||||
rk = Ratekeeper(1, print_delay_threshold=None)
|
||||
|
||||
@@ -7,12 +7,14 @@ import traceback
|
||||
import requests
|
||||
from pathlib import Path
|
||||
from urllib.request import urlopen
|
||||
import openpilot.system.sentry as sentry
|
||||
|
||||
from cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH, MAPD_BIN_DIR
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
from openpilot.system.ui.spinner import Spinner
|
||||
from openpilot.sunnypilot.mapd.mapd_manager import COMMON_DIR, MAPD_PATH, MAPD_BIN_DIR
|
||||
from openpilot.system.version import is_prebuilt
|
||||
import openpilot.system.sentry as sentry
|
||||
|
||||
VERSION = 'v1.9.0'
|
||||
URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
|
||||
@@ -22,28 +24,28 @@ class MapdInstallManager:
|
||||
def __init__(self, spinner_ref: Spinner):
|
||||
self._spinner = spinner_ref
|
||||
|
||||
def download(self):
|
||||
def download(self) -> None:
|
||||
self.ensure_directories_exist()
|
||||
self._download_file()
|
||||
self.update_installed_version(VERSION)
|
||||
|
||||
def check_and_download(self):
|
||||
def check_and_download(self) -> None:
|
||||
if self.download_needed():
|
||||
self.download()
|
||||
|
||||
@staticmethod
|
||||
def download_needed():
|
||||
def download_needed() -> bool:
|
||||
return not os.path.exists(MAPD_PATH) or MapdInstallManager.get_installed_version() != VERSION
|
||||
|
||||
@staticmethod
|
||||
def ensure_directories_exist():
|
||||
if not os.path.exists(COMMON_DIR):
|
||||
os.makedirs(COMMON_DIR)
|
||||
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, content):
|
||||
def _safe_write_and_set_executable(file_path: Path, content: bytes) -> None:
|
||||
with open(file_path, 'wb') as output:
|
||||
output.write(content)
|
||||
output.flush()
|
||||
@@ -51,7 +53,7 @@ class MapdInstallManager:
|
||||
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):
|
||||
def _download_file(self, num_retries=5) -> None:
|
||||
temp_file = Path(MAPD_PATH + ".tmp")
|
||||
download_timeout = 60
|
||||
for cnt in range(num_retries):
|
||||
@@ -75,14 +77,14 @@ class MapdInstallManager:
|
||||
logging.error("Failed to download file after all retries")
|
||||
|
||||
@staticmethod
|
||||
def update_installed_version(version):
|
||||
def update_installed_version(version: str) -> None:
|
||||
Params().put("MapdVersion", version)
|
||||
|
||||
@staticmethod
|
||||
def get_installed_version():
|
||||
def get_installed_version() -> str | None:
|
||||
return Params().get("MapdVersion", encoding="utf-8")
|
||||
|
||||
def wait_for_internet_connection(self, return_on_failure=False):
|
||||
def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool | None:
|
||||
max_retries = 10
|
||||
for retries in range(max_retries + 1):
|
||||
self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
|
||||
@@ -95,7 +97,7 @@ class MapdInstallManager:
|
||||
if return_on_failure and retries == max_retries:
|
||||
return False
|
||||
|
||||
def non_prebuilt_install(self):
|
||||
def non_prebuilt_install(self) -> bool | None:
|
||||
sm = messaging.SubMaster(['deviceState'])
|
||||
metered = sm['deviceState'].networkMetered
|
||||
|
||||
|
||||
@@ -1,38 +1,31 @@
|
||||
import json
|
||||
import time
|
||||
import platform
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate
|
||||
from openpilot.sunnypilot.mapd.live_map_data import QUERY_RADIUS
|
||||
from openpilot.common.realtime import Ratekeeper, set_core_affinity
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
import os
|
||||
import glob
|
||||
import shutil
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
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.sunnypilot.mapd.live_map_data import R
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
# PFEIFER - MAPD {{
|
||||
params = Params()
|
||||
mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
|
||||
# }} PFEIFER - MAPD
|
||||
|
||||
COMMON_DIR = Paths.mapd_root()
|
||||
MAPD_BIN_DIR = os.path.join(BASEDIR, 'third_party/mapd_pfeiferj')
|
||||
MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')
|
||||
|
||||
|
||||
def get_files_for_cleanup():
|
||||
def get_files_for_cleanup() -> list[str]:
|
||||
paths = [
|
||||
f"{COMMON_DIR}/db",
|
||||
f"{COMMON_DIR}/v*"
|
||||
f"{Paths.mapd_root()}/db",
|
||||
f"{Paths.mapd_root()}/v*"
|
||||
]
|
||||
files_to_remove = []
|
||||
for path in paths:
|
||||
@@ -45,7 +38,7 @@ def get_files_for_cleanup():
|
||||
return files_to_remove
|
||||
|
||||
|
||||
def cleanup_OLD_OSM_data(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]):
|
||||
@@ -57,29 +50,6 @@ def cleanup_OLD_OSM_data(files_to_remove):
|
||||
shutil.rmtree(file, ignore_errors=False)
|
||||
|
||||
|
||||
def _get_current_bounding_box(self, radius: float):
|
||||
self.last_query_radius = radius
|
||||
# Calculate the bounding box coordinates for the bbox containing the circle around location.
|
||||
bbox_angle = float(np.degrees(radius / R))
|
||||
|
||||
lat = float(self.last_gps.latitude)
|
||||
lon = float(self.last_gps.longitude)
|
||||
|
||||
return {
|
||||
"min_lat": lat - bbox_angle,
|
||||
"min_lon": lon - bbox_angle,
|
||||
"max_lat": lat + bbox_angle,
|
||||
"max_lon": lon + bbox_angle,
|
||||
}
|
||||
|
||||
|
||||
def _request_refresh_osm_bounds_data(self):
|
||||
self.last_refresh_loc = Coordinate(self.last_gps.latitude, self.last_gps.longitude)
|
||||
self.last_query_radius = QUERY_RADIUS
|
||||
current_bounding_box = self._get_current_bounding_box(self.last_query_radius)
|
||||
mem_params.put("OSMDownloadBounds", json.dumps(current_bounding_box))
|
||||
|
||||
|
||||
def request_refresh_osm_location_data(nations: list[str], states: list[str] = None) -> None:
|
||||
params.put("OsmDownloadedDate", str(time.time()))
|
||||
params.put_bool("OsmDbUpdatesCheck", False)
|
||||
@@ -93,7 +63,7 @@ def request_refresh_osm_location_data(nations: list[str], states: list[str] = No
|
||||
mem_params.put("OSMDownloadLocations", osm_download_locations)
|
||||
|
||||
|
||||
def filter_nations_and_states(nations: list[str], states: list[str] = None):
|
||||
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.
|
||||
@@ -122,11 +92,11 @@ def filter_nations_and_states(nations: list[str], states: list[str] = None):
|
||||
return nations, states or []
|
||||
|
||||
|
||||
def update_osm_db():
|
||||
def update_osm_db() -> None:
|
||||
# last_downloaded_date = float(params.get('OsmDownloadedDate', encoding='utf-8') or 0.0)
|
||||
# if params.get_bool("OsmDbUpdatesCheck") or time.time() - last_downloaded_date >= 604800: # 7 days * 24 hours/day * 60
|
||||
if params.get_bool("OsmDbUpdatesCheck"):
|
||||
cleanup_OLD_OSM_data(get_files_for_cleanup())
|
||||
cleanup_old_osm_data(get_files_for_cleanup())
|
||||
country = params.get('OsmLocationName', encoding='utf-8')
|
||||
state = params.get('OsmStateName', encoding='utf-8') or "All"
|
||||
filtered_nations, filtered_states = filter_nations_and_states([country], [state])
|
||||
@@ -139,21 +109,19 @@ def update_osm_db():
|
||||
mem_params.put("LastGPSPosition", "{}")
|
||||
|
||||
|
||||
def main_thread(sm=None, pm=None):
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("mapd: failed to set core affinity")
|
||||
def main_thread():
|
||||
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(COMMON_DIR)
|
||||
os.mkdir(Paths.mapd_root())
|
||||
except FileExistsError:
|
||||
pass
|
||||
except PermissionError:
|
||||
cloudlog.exception(f"mapd: failed to make {COMMON_DIR}")
|
||||
cloudlog.exception(f"mapd: failed to make {Paths.mapd_root()}")
|
||||
|
||||
while True:
|
||||
show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
|
||||
|
||||
@@ -7,20 +7,14 @@ See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
from cereal import messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
|
||||
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, mpc):
|
||||
self.events_sp = EventsSP()
|
||||
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.slc = SpeedLimitController(CP)
|
||||
|
||||
def get_mpc_mode(self) -> str | None:
|
||||
if not self.dec.active():
|
||||
@@ -28,17 +22,6 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return self.dec.mode()
|
||||
|
||||
def update_v_cruise(self, sm: messaging.SubMaster, long_enabled: bool, v_ego: float, a_ego: float, v_cruise: float) -> float:
|
||||
self.events_sp.clear()
|
||||
|
||||
self.slc.update(long_enabled, v_ego, a_ego, sm, v_cruise, self.events_sp)
|
||||
|
||||
v_cruise_slc = self.slc.speed_limit_offseted if self.slc.is_active else V_CRUISE_UNSET
|
||||
|
||||
v_cruise_final = min(v_cruise, v_cruise_slc)
|
||||
|
||||
return v_cruise_final
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.dec.update(sm)
|
||||
|
||||
@@ -48,7 +31,6 @@ class LongitudinalPlannerSP:
|
||||
plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
||||
|
||||
longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
|
||||
longitudinalPlanSP.events = self.events_sp.to_msg()
|
||||
|
||||
# Dynamic Experimental Control
|
||||
dec = longitudinalPlanSP.dec
|
||||
@@ -56,13 +38,4 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
# Speed Limit Control
|
||||
slc = longitudinalPlanSP.slc
|
||||
slc.state = self.slc.state
|
||||
slc.enabled = self.slc.is_enabled
|
||||
slc.active = self.slc.is_active
|
||||
slc.speedLimit = float(self.slc.speed_limit)
|
||||
slc.speedLimitOffset = float(self.slc.speed_limit_offset)
|
||||
slc.distToSpeedLimit = float(self.slc.distance)
|
||||
|
||||
pm.send('longitudinalPlanSP', plan_sp_send)
|
||||
|
||||
@@ -1,19 +0,0 @@
|
||||
from cereal import custom
|
||||
|
||||
DEBUG = True
|
||||
PARAMS_UPDATE_PERIOD = 2. # secs. Time between parameter updates.
|
||||
TEMP_INACTIVE_GUARD_PERIOD = 1. # secs. Time to wait after activation before considering temp deactivation signal.
|
||||
|
||||
# Lookup table for speed limit percent offset depending on speed.
|
||||
LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h
|
||||
LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h
|
||||
|
||||
# Constants for Limit controllers.
|
||||
LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits.
|
||||
LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide.
|
||||
LIMIT_MAX_ACC = 1.0 # m/s^2 Maximum acceleration allowed for limit controllers to provide while active.
|
||||
LIMIT_MIN_SPEED = 8.33 # m/s, Minimum speed limit to provide as solution on limit controllers.
|
||||
LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and current speed for adapting state.
|
||||
LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers.
|
||||
|
||||
SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState
|
||||
@@ -1,26 +0,0 @@
|
||||
from enum import IntEnum
|
||||
|
||||
|
||||
class Source(IntEnum):
|
||||
none = 0
|
||||
car_state = 1
|
||||
map_data = 2
|
||||
|
||||
|
||||
class Policy(IntEnum):
|
||||
map_data_only = 0
|
||||
car_state_only = 1
|
||||
map_data_priority = 2
|
||||
car_state_priority = 3
|
||||
combined = 4
|
||||
|
||||
|
||||
class Engage(IntEnum):
|
||||
auto = 0
|
||||
user_confirm = 1
|
||||
|
||||
|
||||
class OffsetType(IntEnum):
|
||||
default = 0
|
||||
fixed = 1
|
||||
percentage = 2
|
||||
@@ -1,19 +0,0 @@
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import DEBUG, SpeedLimitControlState
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
|
||||
def debug(msg):
|
||||
if not DEBUG:
|
||||
return
|
||||
cloudlog.debug(msg)
|
||||
|
||||
|
||||
def description_for_state(speed_limit_control_state):
|
||||
if speed_limit_control_state == SpeedLimitControlState.inactive:
|
||||
return 'INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.tempInactive:
|
||||
return 'TEMP_INACTIVE'
|
||||
if speed_limit_control_state == SpeedLimitControlState.adapting:
|
||||
return 'ADAPTING'
|
||||
if speed_limit_control_state == SpeedLimitControlState.active:
|
||||
return 'ACTIVE'
|
||||
@@ -1,344 +0,0 @@
|
||||
from collections.abc import Callable
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
from cereal import messaging, custom
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
|
||||
PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
|
||||
ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting)
|
||||
ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempInactive, *ACTIVE_STATES)
|
||||
|
||||
|
||||
class SpeedLimitController:
|
||||
_speed_limit: float
|
||||
_distance: float
|
||||
_source: Source
|
||||
_v_ego: float
|
||||
_a_ego: float
|
||||
_v_offset: float
|
||||
|
||||
def __init__(self, CP):
|
||||
self._params = Params()
|
||||
self._CP = CP
|
||||
self._policy = self._read_policy_param()
|
||||
self._resolver = SpeedLimitResolver(self._policy)
|
||||
self._last_params_update = 0.0
|
||||
self._last_op_engaged_time = 0.0
|
||||
self._is_metric = self._params.get_bool("IsMetric")
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._op_engaged = False
|
||||
self._op_engaged_prev = False
|
||||
self._v_ego = 0.
|
||||
self._a_ego = 0.
|
||||
self._v_offset = 0.
|
||||
self._v_cruise_setpoint = 0.
|
||||
self._v_cruise_setpoint_prev = 0.
|
||||
self._v_cruise_setpoint_changed = False
|
||||
self._speed_limit = 0.
|
||||
self._speed_limit_prev = 0.
|
||||
self._speed_limit_changed = False
|
||||
self._distance = 0.
|
||||
self._source = Source.none
|
||||
self._state = SpeedLimitControlState.inactive
|
||||
self._state_prev = SpeedLimitControlState.inactive
|
||||
self._gas_pressed = False
|
||||
self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
|
||||
|
||||
self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType"))
|
||||
self._offset_value = self._read_int_param("SpeedLimitValueOffset")
|
||||
self._warning_type = self._read_int_param("SpeedLimitWarningType")
|
||||
self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType"))
|
||||
self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset")
|
||||
self._engage_type = self._read_engage_type_param()
|
||||
self._current_time = 0.
|
||||
self._v_cruise_rounded = 0.
|
||||
self._v_cruise_prev_rounded = 0.
|
||||
self._speed_limit_offsetted_rounded = 0.
|
||||
self._speed_limit_warning_offsetted_rounded = 0.
|
||||
self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
|
||||
|
||||
# Mapping functions to state transitions
|
||||
self.state_transition_strategy = {
|
||||
# Transition functions for each state
|
||||
SpeedLimitControlState.inactive: self.transition_state_from_inactive,
|
||||
SpeedLimitControlState.tempInactive: self.transition_state_from_temp_inactive,
|
||||
SpeedLimitControlState.adapting: self.transition_state_from_adapting,
|
||||
SpeedLimitControlState.active: self.transition_state_from_active,
|
||||
SpeedLimitControlState.preActive: self.transition_state_from_pre_active,
|
||||
}
|
||||
|
||||
# FIXME-SP: unused?
|
||||
# Solution functions mapped to respective states
|
||||
self.acceleration_solutions = {
|
||||
# Solution functions for each state
|
||||
SpeedLimitControlState.tempInactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitControlState.inactive: self.get_current_acceleration_as_target,
|
||||
SpeedLimitControlState.adapting: self.get_adapting_state_target_acceleration,
|
||||
SpeedLimitControlState.active: self.get_active_state_target_acceleration,
|
||||
SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
|
||||
}
|
||||
|
||||
@property
|
||||
def state(self) -> SpeedLimitControlState:
|
||||
return self._state
|
||||
|
||||
@state.setter
|
||||
def state(self, value) -> None:
|
||||
if value != self._state:
|
||||
debug(f'Speed Limit Controller state: {description_for_state(value)}')
|
||||
|
||||
if value == SpeedLimitControlState.tempInactive:
|
||||
# Reset previous speed limit to current value as to prevent going out of tempInactive in
|
||||
# a single cycle when the speed limit changes at the same time the user has temporarily deactivated it.
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
|
||||
self._state = value
|
||||
|
||||
@property
|
||||
def is_enabled(self) -> bool:
|
||||
return self.state in ENABLED_STATES and self._is_enabled
|
||||
|
||||
@property
|
||||
def is_active(self) -> bool:
|
||||
return self.state in ACTIVE_STATES and self._is_enabled
|
||||
|
||||
@property
|
||||
def speed_limit_offseted(self) -> float:
|
||||
return self._speed_limit + self.speed_limit_offset
|
||||
|
||||
@property
|
||||
def speed_limit_offset(self) -> float:
|
||||
return self._get_offset(self._offset_type, self._offset_value)
|
||||
|
||||
@property
|
||||
def speed_limit_warning_offset(self) -> float:
|
||||
return self._get_offset(self._warning_offset_type, self._warning_offset_value)
|
||||
|
||||
@property
|
||||
def speed_limit(self) -> float:
|
||||
return self._speed_limit
|
||||
|
||||
@property
|
||||
def distance(self) -> float:
|
||||
return self._distance
|
||||
|
||||
@property
|
||||
def source(self) -> Source:
|
||||
return self._source
|
||||
|
||||
def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float:
|
||||
if offset_type == OffsetType.default:
|
||||
return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit)
|
||||
elif offset_type == OffsetType.fixed:
|
||||
return offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
|
||||
elif offset_type == OffsetType.percentage:
|
||||
return offset_value * 0.01 * self._speed_limit
|
||||
else:
|
||||
raise NotImplementedError("Offset not supported")
|
||||
|
||||
def _update_v_cruise_setpoint_prev(self) -> None:
|
||||
self._v_cruise_setpoint_prev = self._v_cruise_setpoint
|
||||
|
||||
def _update_params(self) -> None:
|
||||
if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD:
|
||||
self._is_enabled = self._params.get_bool("SpeedLimitControl")
|
||||
self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType"))
|
||||
self._offset_value = self._read_int_param("SpeedLimitValueOffset")
|
||||
self._warning_type = self._read_int_param("SpeedLimitWarningType")
|
||||
self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType"))
|
||||
self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset")
|
||||
self._policy = self._read_policy_param()
|
||||
self._is_metric = self._params.get_bool("IsMetric")
|
||||
self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
|
||||
self._resolver.change_policy(self._policy)
|
||||
self._engage_type = self._read_engage_type_param()
|
||||
|
||||
self._last_params_update = self._current_time
|
||||
|
||||
def _read_policy_param(self) -> Policy:
|
||||
try:
|
||||
return Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8')))
|
||||
except (ValueError, TypeError):
|
||||
return Policy.car_state_priority
|
||||
|
||||
def _read_engage_type_param(self) -> Engage:
|
||||
if self._pcm_cruise_op_long:
|
||||
return Engage.auto
|
||||
|
||||
return Engage(self._read_int_param("SpeedLimitEngageType", validator=lambda x: np.clip(x, Engage.auto, Engage.user_confirm)))
|
||||
|
||||
def _read_int_param(self, key: str, default: int = 0, validator: Callable[[int], int] = None) -> int:
|
||||
try:
|
||||
val = int(self._params.get(key, encoding='utf8'))
|
||||
|
||||
if validator is not None:
|
||||
return validator(val)
|
||||
return val
|
||||
except (ValueError, TypeError):
|
||||
return default
|
||||
|
||||
def _update_calculations(self) -> None:
|
||||
# Update current velocity offset (error)
|
||||
self._v_offset = self.speed_limit_offseted - self._v_ego
|
||||
|
||||
# Track the time op becomes active to prevent going to tempInactive right away after
|
||||
# op enabling since controlsd will change the cruise speed every time on enabling and this will
|
||||
# cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
|
||||
# speed.
|
||||
if not self._op_engaged_prev and self._op_engaged:
|
||||
self._last_op_engaged_time = self._current_time
|
||||
|
||||
# Update change tracking variables
|
||||
self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
|
||||
self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
|
||||
self._speed_limit_prev = self._speed_limit
|
||||
if self._engage_type != Engage.user_confirm:
|
||||
self._update_v_cruise_setpoint_prev()
|
||||
self._op_engaged_prev = self._op_engaged
|
||||
|
||||
self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor))
|
||||
self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor))
|
||||
self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._speed_factor))
|
||||
self._speed_limit_warning_offsetted_rounded = 0 if self._speed_limit == 0 else \
|
||||
int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor))
|
||||
|
||||
def transition_state_from_inactive(self) -> None:
|
||||
""" Make state transition from inactive state """
|
||||
if self._engage_type == Engage.user_confirm:
|
||||
if (((self._last_op_engaged_time + 7.) >= self._current_time >= (self._last_op_engaged_time + 2.)) or
|
||||
self._speed_limit_changed):
|
||||
if self._speed_limit_changed:
|
||||
self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation
|
||||
self.state = SpeedLimitControlState.preActive
|
||||
elif self._engage_type == Engage.auto:
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
else:
|
||||
self.state = SpeedLimitControlState.active
|
||||
|
||||
def transition_state_from_temp_inactive(self) -> None:
|
||||
""" Make state transition from temporary inactive state """
|
||||
if self._speed_limit_changed:
|
||||
if self._engage_type == Engage.user_confirm:
|
||||
self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation
|
||||
self.state = SpeedLimitControlState.preActive
|
||||
elif self._engage_type == Engage.auto:
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
|
||||
def transition_state_from_pre_active(self) -> None:
|
||||
""" Make state transition from preActive state """
|
||||
if self._current_time >= (self._last_op_engaged_time + 7.):
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
elif (self._last_op_engaged_time + 7.) > self._current_time > (self._last_op_engaged_time + 2.):
|
||||
if self._speed_limit_changed:
|
||||
self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation
|
||||
elif self._v_cruise_prev_rounded < self._speed_limit_offsetted_rounded:
|
||||
if self._v_cruise_setpoint > self._v_cruise_setpoint_prev:
|
||||
self.state = SpeedLimitControlState.active
|
||||
elif self._v_cruise_prev_rounded > self._speed_limit_offsetted_rounded:
|
||||
if self._v_cruise_setpoint < self._v_cruise_setpoint_prev:
|
||||
self.state = SpeedLimitControlState.active
|
||||
elif self._v_cruise_prev_rounded == self._speed_limit_offsetted_rounded:
|
||||
self.state = SpeedLimitControlState.active
|
||||
|
||||
def transition_state_from_adapting(self) -> None:
|
||||
""" Make state transition from adapting state """
|
||||
if self._v_offset >= LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.active
|
||||
|
||||
def transition_state_from_active(self) -> None:
|
||||
""" Make state transition from active state """
|
||||
if self._engage_type == Engage.user_confirm:
|
||||
if self._state_prev == SpeedLimitControlState.active:
|
||||
if self._v_cruise_setpoint_changed and self._v_cruise_rounded != self._speed_limit_offsetted_rounded:
|
||||
self.state = SpeedLimitControlState.tempInactive
|
||||
elif self._speed_limit_changed:
|
||||
self._last_op_engaged_time = self._current_time - 2. # immediately prompt confirmation
|
||||
self.state = SpeedLimitControlState.preActive
|
||||
elif self._engage_type == Engage.auto:
|
||||
if self._v_offset < LIMIT_SPEED_OFFSET_TH:
|
||||
self.state = SpeedLimitControlState.adapting
|
||||
|
||||
def _state_transition(self) -> None:
|
||||
self._state_prev = self._state
|
||||
|
||||
# In any case, if op is disabled, or speed limit control is disabled
|
||||
# or the reported speed limit is 0 or gas is pressed, deactivate.
|
||||
if not self._op_engaged or not self._is_enabled or self._speed_limit == 0:
|
||||
self.state = SpeedLimitControlState.inactive
|
||||
return
|
||||
|
||||
# In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
|
||||
# Ignore if a minimum amount of time has not passed since activation. This is to prevent temp inactivations
|
||||
# due to controlsd logic changing cruise setpoint when going active.
|
||||
if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \
|
||||
self._current_time > (self._last_op_engaged_time + TEMP_INACTIVE_GUARD_PERIOD):
|
||||
self.state = SpeedLimitControlState.tempInactive
|
||||
return
|
||||
|
||||
self.state_transition_strategy[self.state]()
|
||||
|
||||
if self._engage_type == Engage.user_confirm:
|
||||
self._update_v_cruise_setpoint_prev()
|
||||
|
||||
def get_current_acceleration_as_target(self) -> float:
|
||||
""" When state is inactive or tempInactive, preserve current acceleration """
|
||||
return self._a_ego
|
||||
|
||||
def get_adapting_state_target_acceleration(self) -> float:
|
||||
""" In adapting state, calculate target acceleration based on speed limit and current velocity """
|
||||
if self.distance > 0:
|
||||
return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)
|
||||
|
||||
return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def get_active_state_target_acceleration(self) -> float:
|
||||
""" In active state, aim to keep speed constant around control time horizon """
|
||||
return self._v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
|
||||
|
||||
def _update_events(self, events_sp: EventsSP) -> None:
|
||||
if self._speed_limit > 0 and self._warning_type == 2 and \
|
||||
self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._speed_factor)):
|
||||
events_sp.add(EventNameSP.speedLimitPreActive)
|
||||
|
||||
if not self.is_active:
|
||||
if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \
|
||||
self._v_cruise_rounded != self._speed_limit_offsetted_rounded:
|
||||
events_sp.add(EventNameSP.speedLimitPreActive)
|
||||
else:
|
||||
if self._engage_type == Engage.user_confirm:
|
||||
if self._state_prev == SpeedLimitControlState.preActive:
|
||||
events_sp.add(EventNameSP.speedLimitConfirmed)
|
||||
events_sp.add(EventNameSP.speedLimitActive)
|
||||
elif self._engage_type == Engage.auto:
|
||||
if self._state_prev not in ACTIVE_STATES:
|
||||
events_sp.add(EventNameSP.speedLimitActive)
|
||||
elif self._speed_limit_changed != 0:
|
||||
events_sp.add(EventNameSP.speedLimitValueChange)
|
||||
|
||||
def update(self, enabled: bool, v_ego: float, a_ego: float, sm: messaging.SubMaster, v_cruise_setpoint: float, events_sp: EventsSP) -> None:
|
||||
_car_state = sm['carState']
|
||||
self._op_engaged = enabled and self._CP.openpilotLongitudinalControl
|
||||
self._v_ego = v_ego
|
||||
self._a_ego = a_ego
|
||||
self._v_cruise_setpoint = v_cruise_setpoint
|
||||
self._gas_pressed = _car_state.gasPressed
|
||||
self._current_time = time.monotonic()
|
||||
|
||||
self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)
|
||||
|
||||
self._update_params()
|
||||
self._update_calculations()
|
||||
self._state_transition()
|
||||
self._update_events(events_sp)
|
||||
@@ -1,125 +0,0 @@
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging, custom
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import debug
|
||||
|
||||
|
||||
class SpeedLimitResolver:
|
||||
_sm: messaging.SubMaster
|
||||
_limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources
|
||||
_distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources
|
||||
_v_ego: float
|
||||
_current_speed_limit: float
|
||||
|
||||
def __init__(self, policy: Policy):
|
||||
self._limit_solutions = {}
|
||||
self._distance_solutions = {}
|
||||
|
||||
self._policy = policy
|
||||
self._policy_to_sources_map = {
|
||||
Policy.car_state_only: [Source.car_state],
|
||||
Policy.car_state_priority: [Source.car_state, Source.map_data],
|
||||
Policy.map_data_priority: [Source.map_data, Source.car_state],
|
||||
Policy.map_data_only: [Source.map_data],
|
||||
Policy.combined: [Source.car_state, Source.map_data],
|
||||
}
|
||||
for source in Source:
|
||||
self._reset_limit_sources(source)
|
||||
|
||||
def change_policy(self, policy: Policy) -> None:
|
||||
self._policy = policy
|
||||
|
||||
def _reset_limit_sources(self, source: Source) -> None:
|
||||
self._limit_solutions[source] = 0.
|
||||
self._distance_solutions[source] = 0.
|
||||
|
||||
def _is_sock_updated(self, sock: str) -> bool:
|
||||
return self._sm.alive[sock] and self._sm.updated[sock]
|
||||
|
||||
def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]:
|
||||
self._v_ego = v_ego
|
||||
self._current_speed_limit = current_speed_limit
|
||||
self._sm = sm
|
||||
|
||||
self._resolve_limit_sources()
|
||||
return self._consolidate()
|
||||
|
||||
def _resolve_limit_sources(self) -> None:
|
||||
"""Get limit solutions from each data source"""
|
||||
self._get_from_car_state()
|
||||
self._get_from_map_data()
|
||||
|
||||
def _get_from_car_state(self) -> None:
|
||||
if not self._is_sock_updated('carStateSP'):
|
||||
debug('SL: No carStateSP instruction for speed limit')
|
||||
return
|
||||
|
||||
self._reset_limit_sources(Source.car_state)
|
||||
self._limit_solutions[Source.car_state] = self._sm['carStateSP'].speedLimit
|
||||
self._distance_solutions[Source.car_state] = 0.
|
||||
|
||||
def _get_from_map_data(self) -> None:
|
||||
if not self._is_sock_updated("liveMapDataSP"):
|
||||
debug('SL: No map data for speed limit')
|
||||
return
|
||||
|
||||
# Load limits from map_data
|
||||
self._reset_limit_sources(Source.map_data)
|
||||
self._process_map_data(self._sm["liveMapDataSP"])
|
||||
|
||||
def _process_map_data(self, map_data: custom.LiveMapDataSP) -> None:
|
||||
gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
|
||||
if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
|
||||
debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
|
||||
return
|
||||
|
||||
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(speed_limit, next_speed_limit, map_data)
|
||||
|
||||
def _calculate_map_data_limits(self, speed_limit: float, next_speed_limit: float, map_data: custom.LiveMapDataSP) -> None:
|
||||
distance_since_fix = self._v_ego * (time.time() - map_data.lastGpsTimestamp * 1e-3)
|
||||
distance_to_speed_limit_ahead = max(0., map_data.speedLimitAheadDistance - distance_since_fix)
|
||||
|
||||
self._limit_solutions[Source.map_data] = speed_limit
|
||||
self._distance_solutions[Source.map_data] = 0.
|
||||
|
||||
if 0. < next_speed_limit < self._v_ego:
|
||||
adapt_time = (next_speed_limit - self._v_ego) / LIMIT_ADAPT_ACC
|
||||
adapt_distance = self._v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time ** 2
|
||||
|
||||
if distance_to_speed_limit_ahead <= adapt_distance:
|
||||
self._limit_solutions[Source.map_data] = next_speed_limit
|
||||
self._distance_solutions[Source.map_data] = distance_to_speed_limit_ahead
|
||||
|
||||
def _consolidate(self) -> tuple[float, float, Source]:
|
||||
source = self._get_source_solution_according_to_policy()
|
||||
self.speed_limit = self._limit_solutions[source] if source else 0.
|
||||
self.distance = self._distance_solutions[source] if source else 0.
|
||||
self.source = source or Source.none
|
||||
|
||||
debug(f'SL: *** Speed Limit set: {self.speed_limit}, distance: {self.distance}, source: {self.source}')
|
||||
return self.speed_limit, self.distance, self.source
|
||||
|
||||
def _get_source_solution_according_to_policy(self) -> Source | None:
|
||||
sources_for_policy = self._policy_to_sources_map[self._policy]
|
||||
|
||||
if self._policy != Policy.combined:
|
||||
# They are ordered in the order of preference, so we pick the first that's non zero
|
||||
for source in sources_for_policy:
|
||||
if self._limit_solutions[source] > 0.:
|
||||
return Source(source)
|
||||
|
||||
limits = np.array([self._limit_solutions[source] for source in sources_for_policy], dtype=float)
|
||||
sources = np.array([source.value for source in sources_for_policy], dtype=int)
|
||||
|
||||
if len(limits) > 0:
|
||||
min_idx = np.argmin(limits)
|
||||
return Source(sources[min_idx])
|
||||
|
||||
return None
|
||||
@@ -1,78 +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
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
|
||||
|
||||
EventNameSP = custom.OnroadEventSP.EventName
|
||||
State = custom.LongitudinalPlanSP.SpeedLimitControlState
|
||||
|
||||
ACTIVE_STATES = (State.active, State.adapting)
|
||||
ENABLED_STATES = (State.preActive, State.tempInactive, *ACTIVE_STATES)
|
||||
|
||||
|
||||
class StateMachine:
|
||||
def __init__(self):
|
||||
self.state = State.inactive
|
||||
|
||||
def update(self, events_sp: EventsSP) -> tuple[bool, bool]:
|
||||
# INACTIVE
|
||||
if self.state == State.inactive:
|
||||
if events_sp.has(EventNameSP.speedLimitEnable):
|
||||
self.state = State.preActive
|
||||
elif events_sp.has(EventNameSP.speedLimitAdapting):
|
||||
self.state = State.adapting
|
||||
elif events_sp.has(EventNameSP.speedLimitActive):
|
||||
self.state = State.active
|
||||
|
||||
# PRE ACTIVE
|
||||
elif self.state == State.preActive:
|
||||
if events_sp.has(EventNameSP.speedLimitDisable):
|
||||
self.state = State.inactive
|
||||
elif events_sp.has(EventNameSP.speedLimitUserCancel):
|
||||
self.state = State.inactive
|
||||
elif events_sp.has(EventNameSP.speedLimitUserConfirm):
|
||||
self.state = State.active
|
||||
elif events_sp.has(EventNameSP.speedLimitActive):
|
||||
self.state = State.active
|
||||
|
||||
# ACTIVE
|
||||
elif self.state == State.active:
|
||||
if events_sp.has(EventNameSP.speedLimitDisable):
|
||||
self.state = State.inactive
|
||||
elif events_sp.has(EventNameSP.speedLimitUserCancel):
|
||||
self.state = State.tempInactive
|
||||
elif events_sp.has(EventNameSP.speedLimitAdapting):
|
||||
self.state = State.adapting
|
||||
elif events_sp.has(EventNameSP.speedLimitValueChange):
|
||||
# For user confirm mode, transition to preActive on speed limit change
|
||||
if events_sp.has(EventNameSP.speedLimitEnable):
|
||||
self.state = State.preActive
|
||||
|
||||
# ADAPTING
|
||||
elif self.state == State.adapting:
|
||||
if events_sp.has(EventNameSP.speedLimitDisable):
|
||||
self.state = State.inactive
|
||||
elif events_sp.has(EventNameSP.speedLimitUserCancel):
|
||||
self.state = State.tempInactive
|
||||
elif events_sp.has(EventNameSP.speedLimitReached):
|
||||
self.state = State.active
|
||||
|
||||
# TEMP INACTIVE
|
||||
elif self.state == State.tempInactive:
|
||||
if events_sp.has(EventNameSP.speedLimitDisable):
|
||||
self.state = State.inactive
|
||||
elif events_sp.has(EventNameSP.speedLimitValueChange):
|
||||
# When speed limit changes, reactivate
|
||||
if events_sp.has(EventNameSP.speedLimitEnable):
|
||||
self.state = State.preActive
|
||||
else:
|
||||
self.state = State.inactive
|
||||
|
||||
enabled = self.state in ENABLED_STATES
|
||||
active = self.state in ACTIVE_STATES
|
||||
|
||||
return enabled, active
|
||||
-128
@@ -1,128 +0,0 @@
|
||||
import random
|
||||
import time
|
||||
|
||||
import pytest
|
||||
from pytest_mock import MockerFixture
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE
|
||||
|
||||
# from selfdrive.controls.lib.speed_limit_controller_tbd import SpeedLimitResolver as OriginalSpeedLimitResolver
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver as RefactoredSpeedLimitResolver
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy
|
||||
|
||||
|
||||
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.,
|
||||
'lastGpsTimestamp': time.time() * 1e3,
|
||||
}, mocker)
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock.__getitem__.side_effect = lambda key: {
|
||||
'carState': car_state,
|
||||
'liveMapDataSP': live_map_data,
|
||||
'carStateSP': car_state_sp,
|
||||
}[key]
|
||||
return sm_mock
|
||||
|
||||
|
||||
parametrized_policies = pytest.mark.parametrize(
|
||||
"policy, sm_key, function_key", [
|
||||
(Policy.car_state_only, 'carStateSP', 'car_state'),
|
||||
(Policy.car_state_priority, 'carStateSP', 'car_state'),
|
||||
(Policy.map_data_only, 'liveMapDataSP', 'map_data'),
|
||||
(Policy.map_data_priority, 'liveMapDataSP', 'map_data'),
|
||||
],
|
||||
ids=lambda val: val.name if hasattr(val, 'name') else str(val)
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("resolver_class", [RefactoredSpeedLimitResolver], ids=["Refactored"])
|
||||
class TestSpeedLimitResolverValidation:
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_initial_state(self, resolver_class, policy):
|
||||
resolver = resolver_class(policy)
|
||||
for source in Source:
|
||||
if source in resolver._limit_solutions:
|
||||
assert resolver._limit_solutions[source] == 0.
|
||||
assert resolver._distance_solutions[source] == 0.
|
||||
|
||||
@parametrized_policies
|
||||
def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the resolver
|
||||
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
|
||||
assert speed_limit == source_speed_limit
|
||||
assert source == Source[function_key]
|
||||
|
||||
def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
|
||||
resolver = resolver_class(Policy.combined)
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
socket_to_source = {'carStateSP': Source.car_state, 'liveMapDataSP': Source.map_data}
|
||||
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
|
||||
speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock)
|
||||
assert speed_limit == minimum_speed_limit
|
||||
assert source == socket_to_source[minimum_key]
|
||||
|
||||
@parametrized_policies
|
||||
def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
source_speed_limit = sm_mock[sm_key].speedLimit
|
||||
|
||||
# Assert the parsing
|
||||
speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock)
|
||||
assert resolver._limit_solutions[Source[function_key]] == source_speed_limit
|
||||
assert resolver._distance_solutions[Source[function_key]] == 0.
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
|
||||
v_ego = 50
|
||||
resolver = resolver_class(policy)
|
||||
|
||||
sm_mock = setup_sm_mock(mocker)
|
||||
_speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock)
|
||||
|
||||
# After resolution
|
||||
assert _speed_limit is not None
|
||||
assert _distance is not None
|
||||
assert _source is not None
|
||||
|
||||
@pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
|
||||
def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
|
||||
resolver = resolver_class(policy)
|
||||
sm_mock = mocker.MagicMock()
|
||||
sm_mock['liveMapDataSP'].lastGpsTimestamp = (time.time() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
|
||||
resolver._sm = sm_mock
|
||||
resolver._get_from_map_data()
|
||||
assert resolver._limit_solutions[Source.map_data] == 0.
|
||||
assert resolver._distance_solutions[Source.map_data] == 0.
|
||||
@@ -1,6 +1,4 @@
|
||||
import cereal.messaging as messaging
|
||||
from cereal import log, car, custom
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
|
||||
NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
|
||||
|
||||
@@ -16,17 +14,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'].slc.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.)
|
||||
|
||||
|
||||
class EventsSP(EventsBase):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
@@ -145,34 +132,6 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
|
||||
|
||||
EventNameSP.pedalPressedAlertOnly: {
|
||||
ET.WARNING: NoEntryAlert("Pedal Pressed")
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitPreActive: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.none, .45), # TODO-SP: AudibleAlert.promptSingleLow
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitActive: {
|
||||
ET.WARNING: Alert(
|
||||
"Set speed changed to match posted speed limit",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.none, 3.),
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitConfirmed: {
|
||||
ET.WARNING: Alert(
|
||||
"",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.none,
|
||||
Priority.MID, VisualAlert.none, AudibleAlert.none, .45), # TODO-SP: AudibleAlert.promptSingleHigh
|
||||
},
|
||||
|
||||
EventNameSP.speedLimitValueChange: {
|
||||
ET.WARNING: speed_limit_adjust_alert,
|
||||
},
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -63,15 +63,6 @@ def manager_init() -> None:
|
||||
("ModelManager_ModelsCache", ""),
|
||||
("NeuralNetworkLateralControl", "0"),
|
||||
("QuietMode", "0"),
|
||||
|
||||
("SpeedLimitControl", "0"),
|
||||
("SpeedLimitControlPolicy", "3"),
|
||||
("SpeedLimitEngageType", "0"),
|
||||
("SpeedLimitOffsetType", "0"),
|
||||
("SpeedLimitValueOffset", "0"),
|
||||
("SpeedLimitWarningType", "0"),
|
||||
("SpeedLimitWarningOffsetType", "0"),
|
||||
("SpeedLimitWarningValueOffset", "0"),
|
||||
]
|
||||
|
||||
if params.get_bool("RecordFrontLock"):
|
||||
|
||||
@@ -6,8 +6,9 @@ from cereal import car, custom
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.system.hardware import PC, TICI
|
||||
from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
|
||||
from openpilot.system.hardware.hw import Paths
|
||||
|
||||
from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH, COMMON_DIR
|
||||
from openpilot.sunnypilot.mapd.mapd_manager import MAPD_PATH
|
||||
|
||||
from sunnypilot.models.helpers import get_active_model_runner
|
||||
from sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, use_sunnylink_uploader
|
||||
@@ -161,7 +162,7 @@ procs += [
|
||||
PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),
|
||||
|
||||
# mapd
|
||||
NativeProcess("mapd", COMMON_DIR, [MAPD_PATH], always_run),
|
||||
NativeProcess("mapd", Paths.mapd_root(), [MAPD_PATH], always_run),
|
||||
PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
|
||||
]
|
||||
|
||||
|
||||
Reference in New Issue
Block a user