diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index df5fbe04f4..4c827aec58 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -211,7 +211,7 @@ class Controls: self.process_not_running = False - self.custom_model, self.model_gen = get_model_generation() + self.custom_model, self.model_gen = get_model_generation(self.params) self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 self.can_log_mono_time = 0 diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index d88d0aa6f1..bd4decbb80 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -62,7 +62,7 @@ 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.custom_model, self.model_gen = get_model_generation(self.param_s) self.model_use_lateral_planner = self.custom_model and self.model_gen == 1 def read_param(self): diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index d56ce6e743..c6e921c6df 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -10,7 +10,6 @@ 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 @@ -30,7 +29,7 @@ STEERING_RATE_COST = 700.0 class LateralPlanner: - def __init__(self, CP, debug=False): + def __init__(self, CP, debug=False, model_use_lateral_planner=False): self.LP = LanePlanner() self.DH = DesireHelper() @@ -74,8 +73,7 @@ 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 + self.model_use_lateral_planner = model_use_lateral_planner def read_param(self): self.dynamic_lane_profile = int(self.param_s.get("DynamicLaneProfile", encoding='utf8')) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 9b3fa8aea8..3c1e8068c5 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -15,9 +15,7 @@ import cereal.messaging as messaging 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, longitudinal_planner): - custom_model, model_gen = get_model_generation() - model_use_lateral_planner = custom_model and model_gen == 1 +def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner, model_use_lateral_planner): 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) @@ -47,7 +45,10 @@ def plannerd_thread(): debug_mode = bool(int(os.getenv("DEBUG", "0"))) longitudinal_planner = LongitudinalPlanner(CP) - lateral_planner = LateralPlanner(CP, debug=debug_mode) + + custom_model, model_gen = get_model_generation(params) + model_use_lateral_planner = custom_model and model_gen == 1 + lateral_planner = LateralPlanner(CP, debug=debug_mode, model_use_lateral_planner=model_use_lateral_planner) lateral_planner_svs = ['lateralPlanDEPRECATED', 'lateralPlanSPDEPRECATED'] pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan', 'longitudinalPlanSP'] + lateral_planner_svs) @@ -64,7 +65,7 @@ def plannerd_thread(): lateral_planner.publish(sm, pm) longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) - publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner) + publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner, model_use_lateral_planner) def main(): plannerd_thread() diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index fb23d34fea..5484c85253 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -4,7 +4,6 @@ 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') @@ -47,8 +46,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, v_ego: float, steer_delay: float, valid: bool) -> None: - custom_model, model_gen = get_model_generation() + nav_enabled: bool, v_ego: float, steer_delay: float, valid: bool, model_use_lateral_planner: bool) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 msg.valid = valid @@ -75,7 +73,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 custom_model and model_gen == 1: + if model_use_lateral_planner: solution = modelV2.lateralPlannerSolutionDEPRECATED 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 fab3021eb7..3aa9a762b8 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -59,23 +59,24 @@ class ModelState: model: ModelRunner def __init__(self, context: CLContext): - self.custom_model, self.model_gen = get_model_generation() + self.param_s = Params() + self.custom_model, self.model_gen = get_model_generation(self.param_s) self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) _inputs = { - 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), # gen2/3 - 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), # gen3 + '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), } if self.custom_model: if self.model_gen == 1: _inputs = { - 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), # gen1 + 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), } - if self.model_gen == 2: # gen2 + if self.model_gen == 2: _inputs = { - 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), # gen2/3 - 'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32), # gen2 + 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), + 'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_LEN, dtype=np.float32), } self.inputs = { @@ -87,8 +88,6 @@ class ModelState: 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } - self.param_s = Params() - if self.param_s.get_bool("CustomDrivingModel"): _model_name = self.param_s.get("DrivingModelText", encoding="utf8") _model_paths = {ModelRunner.THNEED: f"{CUSTOM_MODEL_PATH}/supercombo-{_model_name}.thneed"} @@ -167,8 +166,6 @@ 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) @@ -199,6 +196,7 @@ def main(demo=False): publish_state = PublishState() params = Params() + custom_model, model_gen = get_model_generation(params) 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 @@ -352,7 +350,7 @@ def main(demo=False): 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, v_ego, steer_delay, live_calib_seen) + meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, v_ego, steer_delay, live_calib_seen, custom_model and model_gen == 1) if not (custom_model and model_gen == 1): desire_state = modelv2_send.modelV2.meta.desireState diff --git a/selfdrive/sunnypilot/__init__.py b/selfdrive/sunnypilot/__init__.py index df0dcdbc7f..72d529bc28 100644 --- a/selfdrive/sunnypilot/__init__.py +++ b/selfdrive/sunnypilot/__init__.py @@ -1,8 +1,7 @@ from openpilot.common.params import Params -def get_model_generation(): - params = Params() +def get_model_generation(params=Params()): custom_model = params.get_bool("CustomDrivingModel") gen = int(params.get("DrivingModelGeneration", encoding="utf8")) return custom_model, gen