From 1e5fdf3d1aa639ec56591d1901d78bf49ed0b139 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Fri, 9 Feb 2024 09:54:10 -0500 Subject: [PATCH] Driving Model Selector: Model generation support --- common/params.cc | 1 + selfdrive/controls/controlsd.py | 22 +++++++------ selfdrive/controls/lib/desire_helper.py | 10 +++--- selfdrive/controls/lib/lateral_planner.py | 10 +++--- selfdrive/controls/plannerd.py | 30 +++++++++--------- selfdrive/modeld/fill_model_msg.py | 7 ++--- selfdrive/modeld/modeld.py | 31 ++++++++++--------- selfdrive/sunnypilot/__init__.py | 8 +++++ .../ui/qt/offroad/sunnypilot/models_fetcher.h | 3 ++ .../sunnypilot/software_settings_sp.cc | 1 + 10 files changed, 70 insertions(+), 53 deletions(-) diff --git a/common/params.cc b/common/params.cc index fbd9bd5b84..dfac8877ee 100644 --- a/common/params.cc +++ b/common/params.cc @@ -236,6 +236,7 @@ std::unordered_map keys = { {"DevUIInfo", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DisengageLateralOnBrake", PERSISTENT}, + {"DrivingModelGeneration", PERSISTENT}, {"DrivingModelName", PERSISTENT}, {"DrivingModelText", PERSISTENT}, {"DrivingModelUrl", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 67ef365eeb..cb909ebca6 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -28,6 +28,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.selfdrive.sunnypilot import get_model_generation from openpilot.system.hardware import HARDWARE SOFT_DISABLE_TIME = 3 # seconds @@ -57,8 +58,6 @@ ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) -MODEL_USE_LATERAL_PLANNER = False - class Controls: def __init__(self, CI=None): @@ -211,6 +210,9 @@ class Controls: self.process_not_running = False + self.custom_model, self.model_gen = get_model_generation() + self.model_use_lateral_planner = self.custom_model and self.model_gen == "1" + self.can_log_mono_time = 0 self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) @@ -363,8 +365,8 @@ class Controls: self.events.add(EventName.calibrationInvalid) # Handle lane change - lane_change_edge_block = self.sm['lateralPlanSPDEPRECATED'].laneChangeEdgeBlockDEPRECATED if MODEL_USE_LATERAL_PLANNER else self.sm['modelV2SP'].laneChangeEdgeBlock - lane_change_svs = self.sm['lateralPlanDEPRECATED'] if MODEL_USE_LATERAL_PLANNER else self.sm['modelV2'].meta + lane_change_edge_block = self.sm['lateralPlanSPDEPRECATED'].laneChangeEdgeBlockDEPRECATED if self.model_use_lateral_planner else self.sm['modelV2SP'].laneChangeEdgeBlock + lane_change_svs = self.sm['lateralPlanDEPRECATED'] if self.model_use_lateral_planner else self.sm['modelV2'].meta if lane_change_svs.laneChangeState == LaneChangeState.preLaneChange and lane_change_edge_block: self.events.add(EventName.laneChangeRoadEdge) elif lane_change_svs.laneChangeState == LaneChangeState.preLaneChange: @@ -454,7 +456,7 @@ class Controls: self.logged_comm_issue = None if not (self.CP.notCar and self.joystick_mode): - if MODEL_USE_LATERAL_PLANNER: + if self.model_use_lateral_planner: if not self.sm['lateralPlan'].mpcSolutionValid: self.events.add(EventName.plannerErrorDEPRECATED) if not self.sm['liveLocationKalman'].posenetOK: @@ -671,7 +673,7 @@ class Controls: lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] - blinker_svs = lat_plan if MODEL_USE_LATERAL_PLANNER else model_v2.meta + blinker_svs = lat_plan if self.model_use_lateral_planner else model_v2.meta CC = car.CarControl.new_message() CC.enabled = self.enabled @@ -710,7 +712,7 @@ class Controls: actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - if MODEL_USE_LATERAL_PLANNER: + if self.model_use_lateral_planner: self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures) else: self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature) @@ -719,7 +721,7 @@ class Controls: self.steer_limited, self.desired_curvature, self.sm['liveLocationKalman'], model_data=model_v2) - if MODEL_USE_LATERAL_PLANNER: + if self.model_use_lateral_planner: actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() @@ -759,7 +761,7 @@ class Controls: lac_log.active and self.events.add(EventName.steerSaturated) elif lac_log.saturated: # TODO probably should not use dpath_points but curvature - dpath_points = lat_plan.dPathPoints if MODEL_USE_LATERAL_PLANNER else model_v2.position.y + dpath_points = lat_plan.dPathPoints if self.model_use_lateral_planner else model_v2.position.y if len(dpath_points): # Check if we deviated from the path # TODO use desired vs actual curvature @@ -868,7 +870,7 @@ class Controls: # Curvature & Steering angle lp = self.sm['liveParameters'] - lp_mono_time_svs = 'lateralPlanDEPRECATED' if MODEL_USE_LATERAL_PLANNER else 'modelV2' + lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2' steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d178483f32..57bc7ba963 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -3,6 +3,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.controls.lib.drive_helpers import get_road_edge +from openpilot.selfdrive.sunnypilot import get_model_generation LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -10,8 +11,6 @@ LaneChangeDirection = log.LaneChangeDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. -MODEL_USE_LATERAL_PLANNER = False - DESIRES = { LaneChangeDirection.none: { LaneChangeState.off: log.Desire.none, @@ -63,6 +62,9 @@ class DesireHelper: self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) self.lane_change_bsm_delay = self.param_s.get_bool("AutoLaneChangeBsmDelay") + self.custom_model, self.model_gen = get_model_generation() + self.model_use_lateral_planner = self.custom_model and self.model_gen == "1" + def read_param(self): self.edge_toggle = self.param_s.get_bool("RoadEdge") self.lane_change_set_timer = int(self.param_s.get("AutoLaneChangeTimer", encoding="utf8")) @@ -77,7 +79,7 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - if MODEL_USE_LATERAL_PLANNER: + if self.model_use_lateral_planner: self.road_edge = get_road_edge(carstate, model_data, self.edge_toggle) if not carstate.madsEnabled or self.lane_change_timer > LANE_CHANGE_TIME_MAX: @@ -93,7 +95,7 @@ class DesireHelper: self.lane_change_wait_timer = 0 # LaneChangeState.preLaneChange - elif self.lane_change_state == LaneChangeState.preLaneChange and (self.road_edge if MODEL_USE_LATERAL_PLANNER else lat_plan_sp.laneChangeEdgeBlockDEPRECATED): + elif self.lane_change_state == LaneChangeState.preLaneChange and (self.road_edge if self.model_use_lateral_planner else lat_plan_sp.laneChangeEdgeBlockDEPRECATED): self.lane_change_direction = LaneChangeDirection.none elif self.lane_change_state == LaneChangeState.preLaneChange: # Set lane change direction diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 1fb08884d6..eed82ba75c 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MP from openpilot.selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error, get_road_edge from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from openpilot.selfdrive.sunnypilot import get_model_generation import cereal.messaging as messaging from cereal import log @@ -27,8 +28,6 @@ LATERAL_JERK_COST = 0.04 # speed lateral control is stable on all cars STEERING_RATE_COST = 700.0 -MODEL_USE_LATERAL_PLANNER = False - class LateralPlanner: def __init__(self, CP, debug=False): @@ -75,6 +74,9 @@ class LateralPlanner: self.param_read_counter = 0 self.read_param() + self.custom_model, self.model_gen = get_model_generation() + self.model_use_lateral_planner = self.custom_model and self.model_gen == "1" + def read_param(self): self.dynamic_lane_profile = int(self.param_s.get("DynamicLaneProfile", encoding='utf8')) if self.param_read_counter % 50 == 0: @@ -99,7 +101,7 @@ class LateralPlanner: md = sm['modelV2'] # TODO: SP - Refactor to work with legacy models - if MODEL_USE_LATERAL_PLANNER: + if self.model_use_lateral_planner: self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and (len(md.orientation.x) == TRAJECTORY_SIZE or (len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolutionDEPRECATED.x) == TRAJECTORY_SIZE)): @@ -175,7 +177,7 @@ class LateralPlanner: else: self.solution_invalid_cnt = 0 - if not MODEL_USE_LATERAL_PLANNER: + if not self.model_use_lateral_planner: self.road_edge = get_road_edge(sm['carState'], md, self.edge_toggle) def get_dynamic_lane_profile(self, longitudinal_plan_sp): diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index b4ad81e57e..428bd1f941 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -8,18 +8,18 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner +from openpilot.selfdrive.sunnypilot import get_model_generation import cereal.messaging as messaging -USE_LATERAL_PLANNER = True -MODEL_USE_LATERAL_PLANNER = False - def cumtrapz(x, t): return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))]) -def publish_ui_plan(sm, pm, lateral_planner=None, longitudinal_planner=None): +def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner): # TODO: SP - Reimplement lateral planner with legacy models - if MODEL_USE_LATERAL_PLANNER: + custom_model, model_gen = get_model_generation() + model_use_lateral_planner = custom_model and model_gen == "1" + if model_use_lateral_planner: plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS) model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS) @@ -28,12 +28,12 @@ def publish_ui_plan(sm, pm, lateral_planner=None, longitudinal_planner=None): uiPlan = ui_send.uiPlan uiPlan.frameId = sm['modelV2'].frameId # TODO: SP - Reimplement lateral planner with legacy models - if MODEL_USE_LATERAL_PLANNER: + if model_use_lateral_planner: x_fp = (lateral_planner.x_sol if lateral_planner.dynamic_lane_profile_status else lateral_planner.lat_mpc.x_sol)[:,0] y_fp = (lateral_planner.x_sol if lateral_planner.dynamic_lane_profile_status else lateral_planner.lat_mpc.x_sol)[:,1] - uiPlan.position.x = np.interp(plan_odo, model_odo, x_fp).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.x) - uiPlan.position.y = np.interp(plan_odo, model_odo, y_fp).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.y) - uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() if MODEL_USE_LATERAL_PLANNER else list(sm['modelV2'].position.z) + uiPlan.position.x = np.interp(plan_odo, model_odo, x_fp).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.x) + uiPlan.position.y = np.interp(plan_odo, model_odo, y_fp).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.y) + uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() if model_use_lateral_planner else list(sm['modelV2'].position.z) uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist() pm.send('uiPlan', ui_send) @@ -50,8 +50,8 @@ def plannerd_thread(): longitudinal_planner = LongitudinalPlanner(CP) # TODO: SP - Reimplement lateral planner with legacy models - lateral_planner = LateralPlanner(CP, debug=debug_mode) if USE_LATERAL_PLANNER else None - lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] if USE_LATERAL_PLANNER else [] + lateral_planner = LateralPlanner(CP, debug=debug_mode) + lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP'] + lateral_planner_svs) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', @@ -63,13 +63,11 @@ def plannerd_thread(): sm.update() if sm.updated['modelV2']: - # TODO: SP - Reimplement lateral planner with legacy models - if USE_LATERAL_PLANNER: - lateral_planner.update(sm) - lateral_planner.publish(sm, pm) + lateral_planner.update(sm) + lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - publish_ui_plan(sm, pm, lateral_planner=lateral_planner, longitudinal_planner=longitudinal_planner) + publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner) def main(): plannerd_thread() diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 87a9542cd1..b2cd572a2b 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -4,14 +4,12 @@ import numpy as np from typing import Dict from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta +from openpilot.selfdrive.sunnypilot import get_model_generation SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') ConfidenceClass = log.ModelDataV2.ConfidenceClass -MODEL_USE_LATERAL_PLANNER = False -MODEL_USE_DESIRED_CURVATURES = False - class PublishState: def __init__(self): @@ -50,6 +48,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, 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, v_ego: float, steer_delay: float, valid: bool) -> None: + custom_model, model_gen = get_model_generation() frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 msg.valid = valid @@ -76,7 +75,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) # lateral planning - if MODEL_USE_LATERAL_PLANNER: + if custom_model and model_gen == "1": solution = modelV2.lateralPlannerSolution solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)] diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index e3f4d2c836..2b9146ad91 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -25,6 +25,7 @@ from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext +from openpilot.selfdrive.sunnypilot import get_model_generation PROCESS_NAME = "selfdrive.modeld.modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -39,9 +40,6 @@ CUSTOM_MODEL_PATH = "/data/media/0/models" LaneChangeState = log.LaneChangeState -MODEL_USE_LATERAL_PLANNER = False -MODEL_USE_DESIRED_CURVATURES = False - class FrameMeta: frame_id: int = 0 @@ -61,6 +59,7 @@ class ModelState: model: ModelRunner def __init__(self, context: CLContext): + self.custom_model, self.model_gen = get_model_generation() self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) @@ -74,11 +73,11 @@ class ModelState: 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } - if MODEL_USE_LATERAL_PLANNER: + if self.custom_model and self.model_gen == "1": self.inputs['lat_planner_state'] = np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32) else: self.inputs['lateral_control_params'] = np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32) - if MODEL_USE_DESIRED_CURVATURES: + if self.custom_model and self.model_gen == "2": self.inputs['prev_desired_curvs'] = np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32) else: self.inputs['prev_desired_curv'] = np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32) @@ -121,7 +120,7 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - if not MODEL_USE_LATERAL_PLANNER: + if not (self.custom_model and self.model_gen == "1"): self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -139,10 +138,10 @@ class ModelState: 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 MODEL_USE_LATERAL_PLANNER: + if self.custom_model and self.model_gen == "1": self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2]) self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3]) - elif MODEL_USE_DESIRED_CURVATURES: + elif self.custom_model and self.model_gen == "2": self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] else: @@ -161,6 +160,8 @@ def main(demo=False): model = ModelState(cl_context) cloudlog.warning("models loaded, modeld starting") + custom_model, model_gen = get_model_generation() + # visionipc clients while True: available_streams = VisionIpcClient.available_streams("camerad", block=False) @@ -191,7 +192,7 @@ def main(demo=False): publish_state = PublishState() params = Params() - if not MODEL_USE_LATERAL_PLANNER: + if not (custom_model and model_gen == "1"): with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: steer_delay = msg.steerActuatorDelay + .2 #steer_delay = 0.4 @@ -205,7 +206,7 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - if MODEL_USE_LATERAL_PLANNER: + if custom_model and model_gen == "1": driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], 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) @@ -260,11 +261,11 @@ def main(demo=False): # TODO: path planner timeout? sm.update(0) - desire = sm["lateralPlan"].desire.raw if MODEL_USE_LATERAL_PLANNER else DH.desire + desire = sm["lateralPlan"].desire.raw if custom_model and model_gen == "1" else DH.desire v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId - if not MODEL_USE_LATERAL_PLANNER: + if not (custom_model and model_gen == "1"): # TODO add lag lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"]: @@ -324,7 +325,7 @@ def main(demo=False): 'nav_features': nav_features, 'nav_instructions': nav_instructions} - if MODEL_USE_LATERAL_PLANNER: + if custom_model and model_gen == "1": inputs['driving_style'] = driving_style else: inputs['lateral_control_params'] = lateral_control_params @@ -342,7 +343,7 @@ def main(demo=False): 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, v_ego, steer_delay, live_calib_seen) - if not MODEL_USE_LATERAL_PLANNER: + if not (custom_model and model_gen == "1"): desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] r_lane_change_prob = desire_state[log.Desire.laneChangeRight] @@ -355,7 +356,7 @@ def main(demo=False): pm.send('modelV2', modelv2_send) pm.send('cameraOdometry', posenet_send) - if not MODEL_USE_LATERAL_PLANNER: + if not (custom_model and model_gen == "1"): modelv2_sp_send = messaging.new_message('modelV2SP') modelv2_sp_send.valid = True modelv2_sp_send.modelV2SP.laneChangePrev = DH.prev_lane_change diff --git a/selfdrive/sunnypilot/__init__.py b/selfdrive/sunnypilot/__init__.py index e69de29bb2..9c1e84ca0d 100644 --- a/selfdrive/sunnypilot/__init__.py +++ b/selfdrive/sunnypilot/__init__.py @@ -0,0 +1,8 @@ +from openpilot.common.params import Params + + +def get_model_generation(): + params = Params() + custom_model = params.get_bool("CustomDrivingModel") + gen = params.get("DrivingModelGeneration", encoding="utf8") + return custom_model, gen diff --git a/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.h b/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.h index 1759d3195e..abb6f7e4ce 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.h +++ b/selfdrive/ui/qt/offroad/sunnypilot/models_fetcher.h @@ -28,6 +28,7 @@ public: downloadUri = json["download_uri"].toString(); index = json["index"].toString(); environment = json["environment"].toString(); + generation = json["generation"].toString(); } QJsonObject toJson() const { @@ -38,6 +39,7 @@ public: json["download_uri"] = downloadUri; json["index"] = index; json["environment"] = environment; + json["generation"] = generation; return json; } @@ -47,6 +49,7 @@ public: QString downloadUri; QString index; QString environment; + QString generation; }; class ModelsFetcher : public QObject { diff --git a/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc b/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc index 17875dbc2e..50ba8727e7 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/software_settings_sp.cc @@ -44,6 +44,7 @@ void SoftwarePanelSP::HandleModelDownloadProgressReport() { params.put("DrivingModelText", selectedModelToDownload->fullName.toStdString()); params.put("DrivingModelName", selectedModelToDownload->displayName.toStdString()); //params.put("DrivingModelUrl", selectedModelToDownload->downloadUri.toStdString()); // TODO: Placeholder for future implementations + params.put("DrivingModelGeneration", selectedModelToDownload->generation.toStdString()); selectedModelToDownload.reset(); params.putBool("CustomDrivingModel", true); }