diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0657b3451..393f4fa5f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -101,6 +101,8 @@ class Controls: if REPLAY: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] + if FrogPilotVariables.toggles.radarless_model: + ignore += ['radarState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', @@ -339,8 +341,9 @@ class Controls: self.events.add(EventName.cameraFrameRate) if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) - if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): - self.events.add(EventName.radarFault) + if not self.frogpilot_toggles.radarless_model: + if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): + self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) if CS.canTimeout: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8002ba4ec..2613b44d7 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -9,7 +9,6 @@ from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function from openpilot.selfdrive.car.interfaces import ACCEL_MIN -from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -45,6 +44,8 @@ CRASH_DISTANCE = .25 LEAD_DANGER_FACTOR = 0.75 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI' +# Default lead acceleration decay set to 50% at 1s +LEAD_ACCEL_TAU = 1.5 # Fewer timestamps don't hurt performance and lead to @@ -339,7 +340,7 @@ class LongitudinalMpc: x_lead = 50.0 v_lead = v_ego + 10.0 a_lead = 0.0 - a_lead_tau = _LEAD_ACCEL_TAU + a_lead_tau = LEAD_ACCEL_TAU # MPC will not converge if immediate crash is expected # Clip lead distance to what is still possible to brake for @@ -356,13 +357,13 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, t_follow, trafficModeActive, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): + def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, trafficModeActive, frogpilot_toggles, personality=log.LongitudinalPersonality.standard): v_ego = self.x0[1] - self.status = radarstate.leadOne.status or radarstate.leadTwo.status + self.status = lead_one.status or lead_two.status increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) if not trafficModeActive else 0 - lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance) - lead_xv_1 = self.process_lead(radarstate.leadTwo) + lead_xv_0 = self.process_lead(lead_one, increased_distance) + lead_xv_1 = self.process_lead(lead_two) # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance @@ -421,8 +422,8 @@ class LongitudinalMpc: self.params[:,4] = t_follow self.run() - if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and - radarstate.leadOne.modelProb > 0.9): + lead_probability = lead_one.prob if frogpilot_toggles.radarless_model else lead_one.modelProb + if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and lead_probability > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py old mode 100755 new mode 100644 index 3d61166e1..9d121d225 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -6,14 +6,15 @@ from openpilot.common.numpy_fast import clip, interp import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.simple_kalman import KF1D from openpilot.common.realtime import DT_MDL +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error -from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -25,6 +26,9 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +# Kalman filter states enum +LEAD_KALMAN_SPEED, LEAD_KALMAN_ACCEL = 0, 1 + def get_max_accel(v_ego): return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) @@ -63,6 +67,72 @@ def get_accel_from_plan(CP, speeds, accels): return a_target, should_stop +def lead_kf(v_lead: float, dt: float = 0.05): + # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. + # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s + assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s" + A = [[1.0, dt], [0.0, 1.0]] + C = [1.0, 0.0] + #Q = np.matrix([[10., 0.0], [0.0, 100.]]) + #R = 1e3 + #K = np.matrix([[ 0.05705578], [ 0.03073241]]) + dts = [dt * 0.01 for dt in range(1, 21)] + K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394, + 0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801, + 0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814, + 0.35353899, 0.36200124] + K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219, + 0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714, + 0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557, + 0.26393339, 0.26278425] + K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] + + kf = KF1D([[v_lead], [0.0]], A, C, K) + return kf + + +class Lead: + def __init__(self): + self.dRel = 0.0 + self.yRel = 0.0 + self.vLead = 0.0 + self.aLead = 0.0 + self.vLeadK = 0.0 + self.aLeadK = 0.0 + self.aLeadTau = LEAD_ACCEL_TAU + self.prob = 0.0 + self.status = False + + self.kf: KF1D | None = None + + def reset(self): + self.status = False + self.kf = None + self.aLeadTau = LEAD_ACCEL_TAU + + def update(self, dRel: float, yRel: float, vLead: float, aLead: float, prob: float): + self.dRel = dRel + self.yRel = yRel + self.vLead = vLead + self.aLead = aLead + self.prob = prob + self.status = True + + if self.kf is None: + self.kf = lead_kf(self.vLead) + else: + self.kf.update(self.vLead) + + self.vLeadK = float(self.kf.x[LEAD_KALMAN_SPEED][0]) + self.aLeadK = float(self.kf.x[LEAD_KALMAN_ACCEL][0]) + + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau = LEAD_ACCEL_TAU + else: + self.aLeadTau *= 0.9 + + class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP @@ -74,6 +144,9 @@ class LongitudinalPlanner: self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.v_model_error = 0.0 + self.lead_one = Lead() + self.lead_two = Lead() + self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) @@ -103,6 +176,8 @@ class LongitudinalPlanner: return x, v, a, j def update(self, sm, frogpilot_toggles): + self.secret_good_openpilot = frogpilot_toggles.secretgoodopenpilot_model + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -132,7 +207,7 @@ class LongitudinalPlanner: # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error - self.v_model_error = get_speed_error(sm['modelV2'], v_ego) + self.v_model_error = 0. if self.secret_good_openpilot else get_speed_error(sm['modelV2'], v_ego) if force_slow_decel: v_cruise = 0.0 @@ -140,11 +215,25 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + if frogpilot_toggles.radarless_model: + model_leads = list(sm['modelV2'].leadsV3) + # TODO lead state should be invalidated if its different point than the previous one + lead_states = [self.lead_one, self.lead_two] + for index in range(len(lead_states)): + if len(model_leads) > index: + model_lead = model_leads[index] + lead_states[index].update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) + else: + lead_states[index].reset() + else: + self.lead_one = sm['radarState'].leadOne + self.lead_two = sm['radarState'].leadTwo + self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune) - self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, + self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, sm['frogpilotCarState'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -180,13 +269,19 @@ class LongitudinalPlanner: longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() - longitudinalPlan.hasLead = sm['radarState'].leadOne.status + longitudinalPlan.hasLead = self.lead_one.status longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels) - longitudinalPlan.aTarget = a_target - longitudinalPlan.shouldStop = should_stop + if self.secret_good_openpilot and sm['controlsState'].experimentalMode: + model_speeds = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].velocity.x) + model_accels = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].acceleration.x) + a_target_model, should_stop_model = get_accel_from_plan(self.CP, model_speeds, model_accels) + a_target = min(a_target, a_target_model) + should_stop |= should_stop_model + longitudinalPlan.aTarget = float(a_target) + longitudinalPlan.shouldStop = bool(should_stop) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 613cfbf1d..03a795b9b 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -195,6 +195,8 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn class RadarD: def __init__(self, radar_ts: float, delay: int = 0): + self.points: dict[int, tuple[float, float, float]] = {} + self.current_time = 0.0 self.tracks: dict[int, Track] = {} @@ -206,6 +208,7 @@ class RadarD: self.radar_state: capnp._DynamicStructBuilder | None = None self.radar_state_valid = False + self.radar_tracks_valid = False self.ready = False @@ -257,7 +260,7 @@ class RadarD: self.radar_state.radarErrors = list(radar_errors) self.radar_state.carStateMonoTime = sm.logMonoTime['carState'] - if len(sm['modelV2'].temporalPose.trans): + if len(sm['modelV2'].temporalPose.trans) and not self.frogpilot_toggles.secretgoodopenpilot_model: model_v_ego = sm['modelV2'].temporalPose.trans[0] else: model_v_ego = self.v_ego @@ -294,6 +297,31 @@ class RadarD: } pm.send('liveTracks', tracks_msg) + def update_radardless(self, rr): + radar_points = [] + radar_errors = [] + if rr is not None: + radar_points = rr.points + radar_errors = rr.errors + + self.radar_tracks_valid = len(radar_errors) == 0 + + self.points = {} + for pt in radar_points: + self.points[pt.trackId] = (pt.dRel, pt.yRel, pt.vRel) + + def publish_radardless(self): + tracks_msg = messaging.new_message('liveTracks', len(self.points)) + tracks_msg.valid = self.radar_tracks_valid + for index, tid in enumerate(sorted(self.points.keys())): + tracks_msg.liveTracks[index] = { + "trackId": tid, + "dRel": float(self.points[tid][0]) + RADAR_TO_CAMERA, + "yRel": -float(self.points[tid][1]), + "vRel": float(self.points[tid][2]), + } + + return tracks_msg # fuses camera and radar data for best lead detection def main(): @@ -311,26 +339,41 @@ def main(): # *** setup messaging can_sock = messaging.sub_sock('can') - sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL)) - pm = messaging.PubMaster(['radarState', 'liveTracks']) + pub_sock = messaging.pub_sock('liveTracks') RI = RadarInterface(CP) + # TODO timing is different between cars, need a single time step for all cars + # TODO just take the fastest one for now, and keep resending same messages for slower radars rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) RD = RadarD(CP.radarTimeStep, RI.delay) - while 1: - can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) - rr = RI.update(can_strings) - sm.update(0) - if rr is None: - continue + if not FrogPilotVariables.toggles.radarless_model: + sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL)) + pm = messaging.PubMaster(['radarState', 'liveTracks']) - RD.update(sm, rr) - RD.publish(pm, -rk.remaining*1000.0) + while True: + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) + sm.update(0) + if rr is None: + continue - rk.monitor_time() + RD.update(sm, rr) + RD.publish(pm, -rk.remaining*1000.0) + rk.monitor_time() + else: + while True: + can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + rr = RI.update(can_strings) + if rr is None: + continue + RD.update_radardless(rr) + msg = RD.publish_radardless() + pub_sock.send(msg.to_bytes()) + + rk.monitor_time() if __name__ == "__main__": main() diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index d502db7b3..f7e7ebc14 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -39,6 +39,7 @@ class FrogPilotPlanner: self.params_memory = Params("/dev/shm/params") self.cem = ConditionalExperimentalMode() + self.lead_one = Lead() self.mtsc = MapTurnSpeedController() self.slower_lead = False @@ -53,7 +54,15 @@ class FrogPilotPlanner: self.v_cruise = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles): - self.lead_one = radarState.leadOne + if frogpilot_toggles.radarless_model: + model_leads = list(modelData.leadsV3) + if len(model_leads) > 0: + model_lead = model_leads[0] + self.lead_one.update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob) + else: + self.lead_one.reset() + else: + self.lead_one = radarState.leadOne v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS v_ego = max(carState.vEgo, 0) diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py index 080175985..c0183568e 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py @@ -12,6 +12,7 @@ from openpilot.system.hardware import HARDWARE from openpilot.system.version import get_build_metadata from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import THRESHOLD +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import MODELS_PATH def calculate_lane_width(lane, current_lane, road_edge): current_x, current_y = np.array(current_lane.x), np.array(current_lane.y) @@ -181,6 +182,7 @@ class FrogPilotFunctions: remount_persist = ['sudo', 'mount', '-o', 'remount,rw', '/persist'] run_cmd(remount_persist, "Successfully remounted /persist as read-write.", "Failed to remount /persist.") + os.makedirs(MODELS_PATH, exist_ok=True) os.makedirs("/persist/params", exist_ok=True) frogpilot_boot_logo = f'{BASEDIR}/selfdrive/frogpilot/assets/other_images/frogpilot_boot_logo.png' diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py index f199e5e1a..20d8ebcc0 100644 --- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py +++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.system.version import get_build_metadata -from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import NAVIGATION_MODELS, RADARLESS_MODELS CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive diff --git a/selfdrive/frogpilot/controls/lib/model_manager.py b/selfdrive/frogpilot/controls/lib/model_manager.py new file mode 100644 index 000000000..6cb29ad3d --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/model_manager.py @@ -0,0 +1,164 @@ +import http.client +import os +import socket +import time +import urllib.error +import urllib.request + +from openpilot.common.params import Params +from openpilot.system.version import get_build_metadata + +VERSION = 'v3' if get_build_metadata().channel == "FrogPilot" else 'v4' + +GITHUB_REPOSITORY_URL = 'https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/' +GITLAB_REPOSITORY_URL = 'https://gitlab.com/FrogAi/FrogPilot-Resources/-/raw/' + +DEFAULT_MODEL = "north-dakota-v2" +DEFAULT_MODEL_NAME = "North Dakota V2 (Default)" +MODELS_PATH = '/data/models' + +NAVIGATION_MODELS = {"certified-herbalist", "duck-amigo", "los-angeles", "recertified-herbalist"} +RADARLESS_MODELS = {"radical-turtle"} + +params = Params() +params_memory = Params("/dev/shm/params") + +def ping_url(url, timeout=5): + try: + urllib.request.urlopen(url, timeout=timeout) + return True + except (urllib.error.URLError, socket.timeout, http.client.RemoteDisconnected): + return False + +def determine_url(model, file_type): + if ping_url(GITHUB_REPOSITORY_URL): + return f"{GITHUB_REPOSITORY_URL}/Models/{model}{file_type}" + else: + return f"{GITLAB_REPOSITORY_URL}/Models/{model}{file_type}" + +def delete_deprecated_models(): + populate_models() + + available_models = params.get("AvailableModels", encoding='utf-8').split(',') + + current_model = params.get("Model", block=True, encoding='utf-8') + current_model_file = os.path.join(MODELS_PATH, f"{current_model}.thneed") + + if current_model not in available_models or not os.path.exists(current_model_file): + params.put("Model", DEFAULT_MODEL) + params.put("ModelName", DEFAULT_MODEL_NAME) + + for model_file in os.listdir(MODELS_PATH): + if (model_file.endswith('.thneed') or model_file.endswith('_metadata.pkl')) and model_file[:-7] not in available_models: + os.remove(os.path.join(MODELS_PATH, model_file)) + +def download_model(): + model = params_memory.get("ModelToDownload", encoding='utf-8') + model_path = os.path.join(MODELS_PATH, f"{model}.thneed") + metadata_path = os.path.join(MODELS_PATH, f"{model}_metadata.pkl") + + if os.path.exists(model_path) and os.path.exists(metadata_path): + print(f"Model {model} already exists, skipping download.") + return + + url_thneed = determine_url(model, '.thneed') + url_metadata = determine_url(model, '_metadata.pkl') + + for attempt in range(3): + try: + total_size = get_total_size(url_thneed, url_metadata) + download_file(url_thneed, model_path, 0, total_size) + download_file(url_metadata, metadata_path, os.path.getsize(model_path), total_size) + verify_download(model, model_path, metadata_path) + return + except Exception as e: + handle_download_error(model_path, metadata_path, attempt, e, url_thneed) + time.sleep(2**attempt) + +def get_total_size(url_thneed, url_metadata): + try: + thneed_size = int(urllib.request.urlopen(url_thneed).getheader('Content-Length')) + metadata_size = int(urllib.request.urlopen(url_metadata).getheader('Content-Length')) + return thneed_size + metadata_size + except Exception as e: + print(f"Failed to get total size. Error: {e}") + raise + +def download_file(url, path, progress_start, total_size): + try: + with urllib.request.urlopen(url) as f: + total_file_size = int(f.getheader('Content-Length')) + if total_file_size == 0: + raise ValueError("File is empty") + + with open(path, 'wb') as output: + for chunk in iter(lambda: f.read(8192), b''): + output.write(chunk) + progress = progress_start + output.tell() + params_memory.put_int("ModelDownloadProgress", int((progress / total_size) * 100)) + os.fsync(output) + + except urllib.error.HTTPError as e: + print(f"HTTP Error: {e.code} - {e.reason}") + raise + except urllib.error.URLError as e: + print(f"URL Error: {e.reason}") + raise + except socket.timeout: + print("Socket timeout occurred") + raise + except Exception as e: + print(f"Unexpected error: {e}") + raise + +def verify_download(model, model_path, metadata_path): + total_size = os.path.getsize(model_path) + os.path.getsize(metadata_path) + if total_size == (os.path.getsize(model_path) + os.path.getsize(metadata_path)): + print(f"Successfully downloaded the {model} model and metadata!") + else: + raise Exception("Downloaded file sizes do not match expected sizes.") + +def handle_download_error(model_path, metadata_path, attempt, exception, url): + print(f"Attempt {attempt + 1} failed with error: {exception}. Retrying...") + if os.path.exists(model_path): + os.remove(model_path) + if os.path.exists(metadata_path): + os.remove(metadata_path) + if attempt == 2: + print(f"Failed to download the model after 3 attempts from {url}") + +def populate_models(): + url = f"{GITHUB_REPOSITORY_URL}Versions/model_names_{VERSION}.txt" if ping_url(GITHUB_REPOSITORY_URL) else f"{GITLAB_REPOSITORY_URL}Versions/model_names_{VERSION}.txt" + try: + with urllib.request.urlopen(url) as response: + model_info = [line.decode('utf-8').strip().split(' - ') for line in response.readlines()] + update_params(model_info) + except Exception as e: + print(f"Failed to update models list. Error: {e}") + +def update_params(model_info): + available_models = ','.join(model[0] for model in model_info) + params.put("AvailableModels", available_models) + params.put("AvailableModelsNames", ','.join(model[1] for model in model_info)) + print("Models list updated successfully.") + +def check_metadata(): + for model_file in os.listdir(MODELS_PATH): + if model_file.endswith('.thneed'): + model_name = model_file[:-7] + metadata_file = f"{model_name}_metadata.pkl" + metadata_path = os.path.join(MODELS_PATH, metadata_file) + + if not os.path.exists(metadata_path): + print(f"Metadata for {model_name} is missing, downloading...") + url_metadata = determine_url(model_name, '_metadata.pkl') + + for attempt in range(3): + try: + total_size = int(urllib.request.urlopen(url_metadata).getheader('Content-Length')) + download_file(url_metadata, metadata_path, 0, total_size) + print(f"Successfully downloaded metadata for {model_name}.") + break + except Exception as e: + handle_download_error('', metadata_path, attempt, e, url_metadata) + time.sleep(2**attempt) diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py index cbf9c1e12..9d935d543 100644 --- a/selfdrive/frogpilot/frogpilot_process.py +++ b/selfdrive/frogpilot/frogpilot_process.py @@ -14,6 +14,7 @@ from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, download_model, populate_models WIFI = log.DeviceState.NetworkType.wifi @@ -37,6 +38,8 @@ def automatic_update_check(params): os.system("pkill -SIGUSR1 -f system.updated.updated") def time_checks(automatic_updates, deviceState, maps_downloaded, now, params, params_memory): + populate_models() + screen_off = deviceState.screenBrightnessPercent == 0 wifi_connection = deviceState.networkType == WIFI @@ -96,11 +99,18 @@ def frogpilot_thread(frogpilot_toggles): sm['frogpilotNavigation'], sm['modelV2'], sm['radarState'], frogpilot_toggles) frogpilot_planner.publish(sm, pm, frogpilot_toggles) + if params_memory.get("ModelToDownload", encoding='utf-8') is not None: + download_model() + if FrogPilotVariables.toggles_updated: update_toggles = True elif update_toggles: FrogPilotVariables.update_frogpilot_params(started) + if not frogpilot_toggles.model_selector: + params.put("Model", DEFAULT_MODEL) + params.put("ModelName", DEFAULT_MODEL_NAME) + if time_validated and not started: frogpilot_functions.backup_toggles() diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index dda1ff5e3..7d2b314ac 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -15,7 +15,9 @@ class ModelConstants: # model inputs constants MODEL_FREQ = 20 FEATURE_LEN = 512 + FULL_HISTORY_BUFFER_LEN = 99 HISTORY_BUFFER_LEN = 99 + HISTORY_BUFFER_LEN_SECRET = 24 DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 NAV_FEATURE_LEN = 256 @@ -24,6 +26,7 @@ class ModelConstants: LAT_PLANNER_STATE_LEN = 4 LATERAL_CONTROL_PARAMS_LEN = 2 PREV_DESIRED_CURV_LEN = 1 + RADAR_TRACKS_LEN = 64 # model outputs constants FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) @@ -42,6 +45,7 @@ class ModelConstants: DESIRE_PRED_WIDTH = 8 LAT_PLANNER_SOLUTION_WIDTH = 4 DESIRED_CURV_WIDTH = 1 + RADAR_TRACKS_WIDTH = 3 NUM_LANE_LINES = 4 NUM_ROAD_EDGES = 2 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index c39ec2da3..c14bdaa2b 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -44,7 +44,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, timestamp_eof: int, timestamp_llk: int, model_execution_time: float, - nav_enabled: bool, valid: bool) -> None: + nav_enabled: bool, valid: bool, secret_good_openpilot: bool) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 msg.valid = valid @@ -118,7 +118,10 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, # meta meta = modelV2.meta - meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist() + if secret_good_openpilot: + meta.desireState = np.zeros((ModelConstants.DESIRE_PRED_WIDTH,), dtype=np.float32).reshape(-1).tolist() # TODO + else: + meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist() meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist() meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item() meta.init('disengagePredictions') @@ -141,10 +144,16 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, # temporal pose temporal_pose = modelV2.temporalPose - temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist() - temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist() - temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist() - temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist() + if secret_good_openpilot: + temporal_pose.trans = np.zeros((3,), dtype=np.float32).reshape(-1).tolist() + temporal_pose.transStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist() + temporal_pose.rot = np.zeros((3,), dtype=np.float32).reshape(-1).tolist() + temporal_pose.rotStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist() + else: + temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist() + temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist() + temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist() + temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist() # confidence if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 59ad9ca08..69052e250 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -25,15 +25,26 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, MODELS_PATH PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') +MODEL_NAME = FrogPilotVariables.toggles.model + +DISABLE_NAV = FrogPilotVariables.toggles.navigationless_model +DISABLE_RADAR = FrogPilotVariables.toggles.radarless_model +SECRET_GOOD_OPENPILOT = FrogPilotVariables.toggles.secretgoodopenpilot_model + MODEL_PATHS = { - ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed', + ModelRunner.THNEED: Path(__file__).parent / ('models/supercombo.thneed' if MODEL_NAME == DEFAULT_MODEL else f'{MODELS_PATH}/{MODEL_NAME}.thneed'), ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'} -METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl' +METADATA_PATH = Path(__file__).parent / ('models/supercombo_metadata.pkl' if not SECRET_GOOD_OPENPILOT else f'{MODELS_PATH}/{MODEL_NAME}_metadata.pkl') + +MODEL_WIDTH = 512 +MODEL_HEIGHT = 256 +MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 // 2 class FrameMeta: frame_id: int = 0 @@ -56,14 +67,24 @@ class ModelState: self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) + self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32) self.inputs = { - 'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), + 'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN_SECRET+1 if SECRET_GOOD_OPENPILOT else ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), 'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32), 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), - 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), - 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), + 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN_SECRET+1 if SECRET_GOOD_OPENPILOT else ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), + **({'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32), + 'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)} if not DISABLE_NAV else {}), + 'features_buffer': np.zeros((ModelConstants.HISTORY_BUFFER_LEN_SECRET if SECRET_GOOD_OPENPILOT else ModelConstants.HISTORY_BUFFER_LEN) * ModelConstants.FEATURE_LEN, dtype=np.float32), + **({'radar_tracks': np.zeros(ModelConstants.RADAR_TRACKS_LEN * ModelConstants.RADAR_TRACKS_WIDTH, dtype=np.float32)} if DISABLE_RADAR else {}), } + self.input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32) + self.big_input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32) + self.input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32) + self.big_input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32) + with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) @@ -88,17 +109,45 @@ class ModelState: inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge inputs['desire'][0] = 0 - self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:] - self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) + + if SECRET_GOOD_OPENPILOT: + new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) + self.desire_20Hz[:-1] = self.desire_20Hz[1:] + self.desire_20Hz[-1] = new_desire + self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten() + else: + self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:] + self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0) + self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] + if not DISABLE_NAV: + self.inputs['nav_features'][:] = inputs['nav_features'] + self.inputs['nav_instructions'][:] = inputs['nav_instructions'] + if DISABLE_RADAR: + self.inputs['radar_tracks'][:] = inputs['radar_tracks'] - # if getCLBuffer is not None, frame will be None - self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) - if wbuf is not None: - self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))) + if SECRET_GOOD_OPENPILOT: + new_img = self.frame.prepareSecret(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")) + self.input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.input_imgs_20hz[MODEL_FRAME_SIZE:] + self.input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_img + self.input_imgs[:MODEL_FRAME_SIZE] = self.input_imgs_20hz[:MODEL_FRAME_SIZE] + self.input_imgs[MODEL_FRAME_SIZE:] = self.input_imgs_20hz[-MODEL_FRAME_SIZE:] + self.model.setInputBuffer("input_imgs", self.input_imgs) + if wbuf is not None: + new_big_img = self.wide_frame.prepareSecret(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")) + self.big_input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[MODEL_FRAME_SIZE:] + self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_big_img + self.big_input_imgs[:MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[:MODEL_FRAME_SIZE] + self.big_input_imgs[MODEL_FRAME_SIZE:] = self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:] + self.model.setInputBuffer("big_input_imgs", self.big_input_imgs) + else: + # if getCLBuffer is not None, frame will be None + self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) + if wbuf is not None: + self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))) if prepare_only: return None @@ -106,8 +155,15 @@ class ModelState: self.model.execute() outputs = self.parser.parse_outputs(self.slice_outputs(self.output)) - self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:] - self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :] + if SECRET_GOOD_OPENPILOT: + self.full_features_20Hz[:-1] = self.full_features_20Hz[1:] + self.full_features_20Hz[-1] = outputs['hidden_state'][0, :] + idxs = np.arange(-4,-100,-4)[::-1] + self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten() + else: + self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:] + self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :] + self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:] self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :] return outputs @@ -152,7 +208,7 @@ def main(demo=False, frogpilot_toggles=None): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "liveTracks", "frogpilotPlan"]) publish_state = PublishState() params = Params() @@ -243,7 +299,7 @@ def main(demo=False, frogpilot_toggles=None): # Enable/disable nav features timestamp_llk = sm["navModel"].locationMonoTime nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9) - nav_enabled = nav_valid and params.get_bool("ExperimentalMode") + nav_enabled = nav_valid and not DISABLE_NAV if not nav_enabled: nav_features[:] = 0 @@ -264,6 +320,14 @@ def main(demo=False, frogpilot_toggles=None): if 0 <= distance_idx < 50: nav_instructions[distance_idx*3 + direction_idx] = 1 + radar_tracks = np.zeros(ModelConstants.RADAR_TRACKS_LEN * ModelConstants.RADAR_TRACKS_WIDTH, dtype=np.float32) + if sm.updated["liveTracks"]: + for i, track in enumerate(sm["liveTracks"]): + if i >= ModelConstants.RADAR_TRACKS_LEN: + break + vec_index = i * ModelConstants.RADAR_TRACKS_WIDTH + radar_tracks[vec_index:vec_index+ModelConstants.RADAR_TRACKS_WIDTH] = [track.dRel, track.yRel, track.vRel] + # tracked dropped frames vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) @@ -281,7 +345,9 @@ def main(demo=False, frogpilot_toggles=None): 'desire': vec_desire, 'traffic_convention': traffic_convention, 'lateral_control_params': lateral_control_params, - } + **({'nav_features': nav_features, 'nav_instructions': nav_instructions} if not DISABLE_NAV else {}), + **({'radar_tracks': radar_tracks,} if DISABLE_RADAR else {}), + } mt1 = time.perf_counter() model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) @@ -292,7 +358,7 @@ def main(demo=False, frogpilot_toggles=None): modelv2_send = messaging.new_message('modelV2') posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, - meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen) + meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen, SECRET_GOOD_OPENPILOT) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 523ce00e4..552450f47 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -7,6 +7,7 @@ #include "common/clutil.h" ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) { + frame = std::make_unique(MODEL_FRAME_SIZE); input_frames = std::make_unique(buf_size); q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); @@ -39,6 +40,16 @@ float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int } } +float* ModelFrame::prepareSecret(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) { + transform_queue(&this->transform, q, + yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset, + y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection); + loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl); + CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &frame[0], 0, nullptr, nullptr)); + clFinish(q); + return &frame[0]; +} + ModelFrame::~ModelFrame() { transform_destroy(&transform); loadyuv_destroy(&loadyuv); diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 2cf79094a..0eb7b85fb 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -23,6 +23,7 @@ public: ModelFrame(cl_device_id device_id, cl_context context); ~ModelFrame(); float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output); + float* prepareSecret(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output); const int MODEL_WIDTH = 512; const int MODEL_HEIGHT = 256; @@ -34,5 +35,6 @@ private: LoadYUVState loadyuv; cl_command_queue q; cl_mem y_cl, u_cl, v_cl, net_input_cl; + std::unique_ptr frame; std::unique_ptr input_frames; }; diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index 7c3eb0b3d..c51acfe8f 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -16,5 +16,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h": cppclass ModelFrame: int buf_size + int MODEL_FRAME_SIZE ModelFrame(cl_device_id, cl_context) float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*) + float * prepareSecret(cl_mem, int, int, int, int, mat3, cl_mem*) diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index e292bb0d2..ed9356080 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -45,3 +45,15 @@ cdef class ModelFrame: if not data: return None return np.asarray( data) + + def prepareSecret(self, VisionBuf buf, float[:] projection, CLMem output): + cdef mat3 cprojection + memcpy(cprojection.v, &projection[0], 9*sizeof(float)) + cdef float * data + if output is None: + data = self.frame.prepareSecret(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL) + else: + data = self.frame.prepareSecret(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem) + if not data: + return None + return np.asarray( data) diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index af57e11d0..b2e68cca2 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -1,6 +1,8 @@ import numpy as np from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + def sigmoid(x): return 1. / (1. + np.exp(-x)) @@ -17,6 +19,9 @@ class Parser: def __init__(self, ignore_missing=False): self.ignore_missing = ignore_missing + # FrogPilot variables + self.secret_good_openpilot = FrogPilotVariables.toggles.secretgoodopenpilot_model + def check_missing(self, outs, name): if name not in outs and not self.ignore_missing: raise ValueError(f"Missing output {name}") @@ -88,7 +93,8 @@ class Parser: self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) - self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) + if not self.secret_good_openpilot: + self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,)) self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION, out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH)) @@ -98,6 +104,7 @@ class Parser: self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,)) for k in ['lead_prob', 'lane_lines_prob', 'meta']: self.parse_binary_crossentropy(k, outs) - self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) + if not self.secret_good_openpilot: + self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) return outs diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index e711e73f8..59e42b234 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -239,8 +239,14 @@ void OffroadHome::hideEvent(QHideEvent *event) { } void OffroadHome::refresh() { + QString model = QString::fromStdString(params.get("ModelName")); + + if (model.contains("(Default)")) { + model = model.remove("(Default)").trimmed(); + } + date->setText(QLocale(uiState()->language.mid(5)).toString(QDateTime::currentDateTime(), "dddd, MMMM d")); - version->setText(getBrand() + " v" + getVersion().left(14).trimmed()); + version->setText(getBrand() + " v" + getVersion().left(14).trimmed() + " - " + model); bool updateAvailable = update_widget->refresh(); int alerts = alerts_widget->refresh(); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 29c212689..22877b4f9 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -332,13 +332,13 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) painter.restore(); } -void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) { +void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego) { painter.save(); const float speedBuff = 10.; const float leadBuff = 40.; - const float d_rel = lead_data.getDRel(); - const float v_rel = lead_data.getVRel(); + const float d_rel = lead_data.getX()[0]; + const float v_rel = lead_data.getV()[0] - v_ego; float fillAlpha = 0; if (d_rel < leadBuff) { @@ -373,6 +373,7 @@ void AnnotatedCameraWidget::paintGL() { SubMaster &sm = *(s->sm); const double start_draw_t = millis_since_boot(); const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2(); + const float v_ego = sm["carState"].getCarState().getVEgo(); // draw camera frame { @@ -394,7 +395,6 @@ void AnnotatedCameraWidget::paintGL() { // Wide or narrow cam dependent on speed bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD); if (has_wide_cam) { - float v_ego = sm["carState"].getCarState().getVEgo(); if ((v_ego < 10) || available_streams.size() == 1) { wide_cam_requested = true; } else if (v_ego > 15) { @@ -425,16 +425,16 @@ void AnnotatedCameraWidget::paintGL() { update_model(s, model, sm["uiPlan"].getUiPlan()); drawLaneLines(painter, s); - if (s->scene.longitudinal_control && sm.rcv_frame("radarState") > s->scene.started_frame) { - auto radar_state = sm["radarState"].getRadarState(); - update_leads(s, radar_state, model.getPosition()); - auto lead_one = radar_state.getLeadOne(); - auto lead_two = radar_state.getLeadTwo(); - if (lead_one.getStatus()) { - drawLead(painter, lead_one, s->scene.lead_vertices[0]); - } - if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) { - drawLead(painter, lead_two, s->scene.lead_vertices[1]); + if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) { + update_leads(s, model); + float prev_drel = -1; + for (int i = 0; i < model.getLeadsV3().size() && i < 2; i++) { + const auto &lead = model.getLeadsV3()[i]; + auto lead_drel = lead.getX()[0]; + if (s->scene.has_lead && (prev_drel < 0 || std::abs(lead_drel - prev_drel) > 3.0)) { + drawLead(painter, lead, s->scene.lead_vertices[i], v_ego); + } + prev_drel = lead_drel; } } } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 8f5ffc73b..094561093 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -84,7 +84,7 @@ protected: void showEvent(QShowEvent *event) override; void updateFrameMat() override; void drawLaneLines(QPainter &painter, const UIState *s); - void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); + void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego); void drawHud(QPainter &p); void drawDriverState(QPainter &painter, const UIState *s); inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 769bb5ece..4e14ef2ae 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -44,12 +44,15 @@ int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_h return max_idx; } -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) { - for (int i = 0; i < 2; ++i) { - auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo(); - if (lead_data.getStatus()) { - float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())]; - calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]); +void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data) { + const cereal::XYZTData::Reader &line = model_data.getPosition(); + for (int i = 0; i < model_data.getLeadsV3().size() && i < 2; ++i) { + const auto &lead = model_data.getLeadsV3()[i]; + if (s->scene.has_lead) { + float d_rel = lead.getX()[0]; + float y_rel = lead.getY()[0]; + float z = line.getZ()[get_path_length_idx(line, d_rel)]; + calib_frame_to_full_frame(s, d_rel, y_rel, z + 1.22, &s->scene.lead_vertices[i]); } } } @@ -105,10 +108,14 @@ void update_model(UIState *s, } // update path - auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne(); - if (lead_one.getStatus()) { - const float lead_d = lead_one.getDRel() * 2.; - max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); + auto lead_count = model.getLeadsV3().size(); + if (lead_count > 0) { + auto lead_one = model.getLeadsV3()[0]; + scene.has_lead = lead_one.getProb() > scene.lead_detection_threshold; + if (scene.has_lead) { + const float lead_d = lead_one.getX()[0] * 2.; + max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); + } } max_idx = get_path_length_idx(plan_position, max_distance); update_line_data(s, plan_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); @@ -287,6 +294,10 @@ void ui_update_frogpilot_params(UIState *s) { scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap"); + bool longitudinal_tune = scene.longitudinal_control && params.getBool("LongitudinalTune"); + bool radarless_model = params.get("Model") == "radical-turtle"; + scene.lead_detection_threshold = longitudinal_tune && !radarless_model ? params.getInt("LeadDetectionThreshold") / 100.0f : 0.5; + scene.tethering_config = params.getInt("TetheringEnabled"); if (scene.tethering_config == 2) { WifiManager(s).setTetheringEnabled(true); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 3c6bcdac5..6e869824d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -127,6 +127,7 @@ typedef struct UIScene { bool enabled; bool experimental_mode; bool experimental_mode_via_screen; + bool has_lead; bool map_open; bool online; bool onroad_distance_button; @@ -140,6 +141,7 @@ typedef struct UIScene { bool use_kaofui_icons; float adjusted_cruise; + float lead_detection_threshold; int alert_size; int conditional_speed; @@ -239,7 +241,7 @@ void update_model(UIState *s, const cereal::ModelDataV2::Reader &model, const cereal::UiPlan::Reader &plan); void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd); -void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line); +void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data); void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert); diff --git a/system/manager/manager.py b/system/manager/manager.py index bef855c2d..a534a1e67 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -23,6 +23,7 @@ from openpilot.common.time import system_time_valid from openpilot.system.version import get_build_metadata, terms_version, training_version from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions +from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, check_metadata, delete_deprecated_models def frogpilot_boot_functions(frogpilot_functions): @@ -42,6 +43,18 @@ def frogpilot_boot_functions(frogpilot_functions): print(f"Failed to backup toggles. Error: {e}") return + try: + delete_deprecated_models() + except subprocess.CalledProcessError as e: + print(f"Failed to delete deprecated models. Error: {e}") + return + + try: + check_metadata() + except subprocess.CalledProcessError as e: + print(f"Failed to check metadata models. Error: {e}") + return + def manager_init(frogpilot_functions) -> None: frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,)) frogpilot_boot.start()