diff --git a/.gitignore b/.gitignore index afb3d5b1c3..e414e4301a 100644 --- a/.gitignore +++ b/.gitignore @@ -79,6 +79,7 @@ comma*.sh selfdrive/modeld/thneed/compile selfdrive/modeld/models/*.thneed +selfdrive/modeld/models/*.pkl *.bz2 diff --git a/release/files_common b/release/files_common index ad85a6a21a..05bd463463 100644 --- a/release/files_common +++ b/release/files_common @@ -358,6 +358,9 @@ selfdrive/modeld/.gitignore selfdrive/modeld/__init__.py selfdrive/modeld/SConscript selfdrive/modeld/modeld.py +selfdrive/modeld/parse_model_outputs.py +selfdrive/modeld/fill_model_msg.py +selfdrive/modeld/get_model_metadata.py selfdrive/modeld/navmodeld.py selfdrive/modeld/dmonitoringmodeld.py selfdrive/modeld/constants.py @@ -370,8 +373,6 @@ selfdrive/modeld/models/*.pyx selfdrive/modeld/models/commonmodel.cc selfdrive/modeld/models/commonmodel.h -selfdrive/modeld/models/driving.cc -selfdrive/modeld/models/driving.h selfdrive/modeld/models/supercombo.onnx selfdrive/modeld/models/dmonitoring_model_q.dlc diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 00916ddf78..b728f1114c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -4,7 +4,7 @@ from cereal import car, log from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.modeld.constants import T_IDXS +from openpilot.selfdrive.modeld.constants import ModelConstants # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable @@ -177,7 +177,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): # in high delay cases some corrections never even get commanded. So just use # psi to calculate a simple linearization of desired curvature current_curvature_desired = curvatures[0] - psi = interp(delay, T_IDXS[:CONTROL_N], psis) + psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis) average_curvature_desired = psi / (v_ego * delay) desired_curvature = 2 * average_curvature_desired - current_curvature_desired diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 5e6f884df4..83ec3b3a13 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -5,7 +5,7 @@ import numpy as np from casadi import SX, vertcat, sin, cos # WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import T_IDXS +from openpilot.selfdrive.modeld.constants import ModelConstants if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -66,7 +66,7 @@ def gen_lat_ocp(): ocp = AcadosOcp() ocp.model = gen_lat_model() - Tf = np.array(T_IDXS)[N] + Tf = np.array(ModelConstants.T_IDXS)[N] # set dimensions ocp.dims.N = N @@ -122,7 +122,7 @@ def gen_lat_ocp(): # set prediction horizon ocp.solver_options.tf = Tf - ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1] + ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1] ocp.code_export_directory = EXPORT_DIR return ocp diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 61c150aadc..ee65c4a69e 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone from openpilot.selfdrive.controls.lib.pid import PIDController -from openpilot.selfdrive.modeld.constants import T_IDXS +from openpilot.selfdrive.modeld.constants import ModelConstants LongCtrlState = car.CarControl.Actuators.LongControlState @@ -70,19 +70,19 @@ class LongControl: # Interp control trajectory speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds) - a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels) + v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) + a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) - v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now - v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds) + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) - v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds) + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds) else: v_target = 0.0 v_target_now = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c1e782fa5c..b1eff5302f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,7 +9,7 @@ import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL -from openpilot.selfdrive.modeld.constants import T_IDXS +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 @@ -76,9 +76,9 @@ class LongitudinalPlanner: if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): - x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC - v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error - a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x) + x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC + v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error + a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) j = np.zeros(len(T_IDXS_MPC)) else: x = np.zeros(len(T_IDXS_MPC)) @@ -135,11 +135,11 @@ class LongitudinalPlanner: x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) - self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) - self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) + self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] - self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) + self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill @@ -148,7 +148,7 @@ class LongitudinalPlanner: # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired - self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) + self.a_desired = float(interp(DT_MDL, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index ed1a2fa09a..1965bf8588 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -5,7 +5,7 @@ from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process from openpilot.system.swaglog import cloudlog -from openpilot.selfdrive.modeld.constants import T_IDXS +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 import cereal.messaging as messaging @@ -14,8 +14,8 @@ 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): - plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS) - model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS) + plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS) + model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS) ui_send = messaging.new_message('uiPlan') ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2']) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 509c4e6aa7..286f3a0a4a 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -45,17 +45,19 @@ snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else sn cython_libs = envCython["LIBS"] + libs snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc']) commonmodel_lib = lenv.Library('commonmodel', common_src) -driving_lib = lenv.Library('driving', ['models/driving.cc']) lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks) lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath) lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) -lenvCython.Program('models/driving_pyx.so', 'models/driving_pyx.pyx', LIBS=[driving_lib, commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) + +# Get model metadata +fn = File("models/supercombo").abspath +cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx' +files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.endswith("get_model_metadata.py")], []) +lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"]+files, cmd) # Build thneed model if arch == "larch64" or GetOption('pc_thneed'): - fn = File("models/supercombo").abspath - tinygrad_opts = ["NOLOCALS=1", "IMAGE=2", "GPU=1"] if not GetOption('pc_thneed'): # use FLOAT16 on device for speed + don't cache the CL kernels for space diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 125864b98b..a5ac977390 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -1,7 +1,78 @@ -IDX_N = 33 +import numpy as np def index_function(idx, max_val=192, max_idx=32): return (max_val) * ((idx/max_idx)**2) +class ModelConstants: + # time and distance indices + IDX_N = 33 + T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] + X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)] + LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] + LEAD_T_OFFSETS = [0., 2., 4.] + META_T_IDXS = [2., 4., 6., 8., 10.] -T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] + # model inputs constants + MODEL_FREQ = 20 + FEATURE_LEN = 512 + HISTORY_BUFFER_LEN = 99 + DESIRE_LEN = 8 + TRAFFIC_CONVENTION_LEN = 2 + NAV_FEATURE_LEN = 256 + NAV_INSTRUCTION_LEN = 150 + DRIVING_STYLE_LEN = 12 + + # model outputs constants + FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) + FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) + + DISENGAGE_WIDTH = 5 + POSE_WIDTH = 6 + WIDE_FROM_DEVICE_WIDTH = 3 + SIM_POSE_WIDTH = 6 + LEAD_WIDTH = 4 + LANE_LINES_WIDTH = 2 + ROAD_EDGES_WIDTH = 2 + PLAN_WIDTH = 15 + DESIRE_PRED_WIDTH = 8 + + NUM_LANE_LINES = 4 + NUM_ROAD_EDGES = 2 + + LEAD_TRAJ_LEN = 6 + DESIRE_PRED_LEN = 4 + + PLAN_MHP_N = 5 + LEAD_MHP_N = 2 + PLAN_MHP_SELECTION = 1 + LEAD_MHP_SELECTION = 3 + + FCW_THRESHOLD_5MS2_HIGH = 0.15 + FCW_THRESHOLD_5MS2_LOW = 0.05 + FCW_THRESHOLD_3MS2 = 0.7 + + CONFIDENCE_BUFFER_LEN = 5 + RYG_GREEN = 0.01165 + RYG_YELLOW = 0.06157 + +# model outputs slices +class Plan: + POSITION = slice(0, 3) + VELOCITY = slice(3, 6) + ACCELERATION = slice(6, 9) + T_FROM_CURRENT_EULER = slice(9, 12) + ORIENTATION_RATE = slice(12, 15) + +class Meta: + ENGAGED = slice(0, 1) + # next 2, 4, 6, 8, 10 seconds + GAS_DISENGAGE = slice(1, 36, 7) + BRAKE_DISENGAGE = slice(2, 36, 7) + STEER_OVERRIDE = slice(3, 36, 7) + HARD_BRAKE_3 = slice(4, 36, 7) + HARD_BRAKE_4 = slice(5, 36, 7) + HARD_BRAKE_5 = slice(6, 36, 7) + GAS_PRESS = slice(7, 36, 7) + # next 0, 2, 4, 6, 8, 10 seconds + LEFT_BLINKER = slice(36, 48, 2) + RIGHT_BLINKER = slice(37, 48, 2) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py new file mode 100644 index 0000000000..dda2e41258 --- /dev/null +++ b/selfdrive/modeld/fill_model_msg.py @@ -0,0 +1,181 @@ +import capnp +import numpy as np +from typing import Dict +from cereal import log +from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta + +ConfidenceClass = log.ModelDataV2.ConfidenceClass + +class PublishState: + def __init__(self): + self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) + self.prev_brake_5ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) + self.prev_brake_3ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) + +def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None): + builder.t = t + builder.x = x.tolist() + builder.y = y.tolist() + builder.z = z.tolist() + if x_std is not None: + builder.xStd = x_std.tolist() + if y_std is not None: + builder.yStd = y_std.tolist() + if z_std is not None: + builder.zStd = z_std.tolist() + +def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None): + builder.t = t + builder.x = x.tolist() + builder.y = y.tolist() + builder.v = v.tolist() + builder.a = a.tolist() + if x_std is not None: + builder.xStd = x_std.tolist() + if y_std is not None: + builder.yStd = y_std.tolist() + if v_std is not None: + builder.vStd = v_std.tolist() + if a_std is not None: + builder.aStd = a_std.tolist() + +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: + frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 + msg.valid = valid + + modelV2 = msg.modelV2 + modelV2.frameId = vipc_frame_id + modelV2.frameIdExtra = vipc_frame_id_extra + modelV2.frameAge = frame_age + modelV2.frameDropPerc = frame_drop * 100 + modelV2.timestampEof = timestamp_eof + modelV2.locationMonoTime = timestamp_llk + modelV2.modelExecutionTime = model_execution_time + modelV2.navEnabled = nav_enabled + + # plan + position = modelV2.position + fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T) + velocity = modelV2.velocity + fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T) + acceleration = modelV2.acceleration + fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T) + orientation = modelV2.orientation + fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T) + orientation_rate = modelV2.orientationRate + fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T) + + # times at X_IDXS according to model plan + PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N + PLAN_T_IDXS[0] = 0.0 + plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() + for xidx in range(1, ModelConstants.IDX_N): + tidx = 0 + # increment tidx until we find an element that's further away than the current xidx + while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: + tidx += 1 + if tidx == ModelConstants.IDX_N - 1: + # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break + PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] + break + # interpolate to find `t` for the current xidx + current_x_val = plan_x[tidx] + next_x_val = plan_x[tidx+1] + p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) + PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] + + # lane lines + modelV2.init('laneLines', 4) + for i in range(4): + lane_line = modelV2.laneLines[i] + fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() + modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() + + # road edges + modelV2.init('roadEdges', 2) + for i in range(2): + road_edge = modelV2.roadEdges[i] + fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) + modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist() + + # leads + modelV2.init('leadsV3', 3) + for i in range(3): + lead = modelV2.leadsV3[i] + fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T) + lead.prob = net_output_data['lead_prob'][0,i].tolist() + lead.probTime = ModelConstants.LEAD_T_OFFSETS[i] + + # meta + meta = modelV2.meta + 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') + disengage_predictions = meta.disengagePredictions + disengage_predictions.t = ModelConstants.META_T_IDXS + disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist() + disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist() + disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist() + disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist() + disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist() + disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist() + + publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:] + publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0] + publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:] + publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0] + hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \ + (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all() + meta.hardBrakePredicted = hard_brake_predicted.item() + + # 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() + + # confidence + if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0: + # any disengage prob + brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE] + gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE] + steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE] + any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs)) + # independent disengage prob for each 2s slice + ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])] + # rolling buf for 2, 4, 6, 8, 10s + publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:] + publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs + + score = 0. + for i in range(ModelConstants.DISENGAGE_WIDTH): + score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH + if score < ModelConstants.RYG_GREEN: + modelV2.confidence = ConfidenceClass.green + elif score < ModelConstants.RYG_YELLOW: + modelV2.confidence = ConfidenceClass.yellow + else: + modelV2.confidence = ConfidenceClass.red + +def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], + vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: + msg.valid = live_calib_seen & (vipc_dropped_frames < 1) + cameraOdometry = msg.cameraOdometry + + cameraOdometry.frameId = vipc_frame_id + cameraOdometry.timestampEof = timestamp_eof + + cameraOdometry.trans = net_output_data['pose'][0,:3].tolist() + cameraOdometry.rot = net_output_data['pose'][0,3:].tolist() + cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist() + cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist() + cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist() + cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist() + cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist() + cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist() diff --git a/selfdrive/modeld/get_model_metadata.py b/selfdrive/modeld/get_model_metadata.py new file mode 100755 index 0000000000..187f83399b --- /dev/null +++ b/selfdrive/modeld/get_model_metadata.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +import sys +import pathlib +import onnx +import codecs +import pickle +from typing import Tuple + +def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]: + shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) + name = value_info.name + return name, shape + +if __name__ == "__main__": + model_path = pathlib.Path(sys.argv[1]) + model = onnx.load(str(model_path)) + i = [x.key for x in model.metadata_props].index('output_slices') + output_slices = model.metadata_props[i].value + + metadata = {} + metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64")) + metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input]) + metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output]) + + metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl') + with open(metadata_path, 'wb') as f: + pickle.dump(metadata, f) + + print(f'saved metadata to {metadata_path}') diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 4f49398797..8bfdaaa350 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 import sys import time +import pickle import numpy as np +import cereal.messaging as messaging from pathlib import Path from typing import Dict, Optional from setproctitle import setproctitle @@ -13,16 +15,17 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime +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.modeld.models.driving_pyx import ( - PublishState, create_model_msg, create_pose_msg, - FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN, - OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ) MODEL_PATHS = { ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed', ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'} +METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl' + class FrameMeta: frame_id: int = 0 timestamp_sof: int = 0 @@ -43,28 +46,38 @@ class ModelState: def __init__(self, context: CLContext): self.frame = ModelFrame(context) self.wide_frame = ModelFrame(context) - self.prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32) - self.output = np.zeros(NET_OUTPUT_SIZE, dtype=np.float32) + self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) self.inputs = { - 'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32), - 'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32), - 'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32), - 'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32), - 'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32), + 'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), + 'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, 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), + 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } + with open(METADATA_PATH, 'rb') as f: + model_metadata = pickle.load(f) + + self.output_slices = model_metadata['output_slices'] + net_output_size = model_metadata['output_shapes']['outputs'][1] + self.output = np.zeros(net_output_size, dtype=np.float32) + self.parser = Parser() + self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context) self.model.addInput("input_imgs", None) self.model.addInput("big_input_imgs", None) for k,v in self.inputs.items(): self.model.addInput(k, v) + def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]: + return {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} + def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, - inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[np.ndarray]: + inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge inputs['desire'][0] = 0 - self.inputs['desire'][:-DESIRE_LEN] = self.inputs['desire'][DESIRE_LEN:] - self.inputs['desire'][-DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 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) self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] @@ -81,9 +94,11 @@ class ModelState: return None self.model.execute() - self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:] - self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN] - return self.output + 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, :] + return outputs def main(): @@ -122,22 +137,21 @@ def main(): pm = PubMaster(["modelV2", "cameraOdometry"]) sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"]) - state = PublishState() + publish_state = PublishState() params = Params() # setup filter to track dropped frames - frame_dropped_filter = FirstOrderFilter(0., 10., 1. / MODEL_FREQ) + frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) frame_id = 0 last_vipc_frame_id = 0 run_count = 0 - # last = 0.0 model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32) - nav_features = np.zeros(NAV_FEATURE_LEN, dtype=np.float32) - nav_instructions = np.zeros(NAV_INSTRUCTION_LEN, 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) buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() @@ -190,8 +204,8 @@ def main(): traffic_convention = np.zeros(2) traffic_convention[int(is_rhd)] = 1 - vec_desire = np.zeros(DESIRE_LEN, dtype=np.float32) - if desire >= 0 and desire < DESIRE_LEN: + vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) + if desire >= 0 and desire < ModelConstants.DESIRE_LEN: vec_desire[desire] = 1 # Enable/disable nav features @@ -244,13 +258,15 @@ def main(): model_execution_time = mt2 - mt1 if model_output is not None: - pm.send("modelV2", create_model_msg(model_output, 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)) - pm.send("cameraOdometry", create_pose_msg(model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)) + 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) + + fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) + pm.send('modelV2', modelv2_send) + pm.send('cameraOdometry', posenet_send) - # print("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f" % - # ((mt2 - mt1)*1000, (mt1 - last)*1000, meta_extra.frame_id, frame_id, frame_drop_ratio)) - # last = mt1 last_vipc_frame_id = meta_main.frame_id diff --git a/selfdrive/modeld/models/README.md b/selfdrive/modeld/models/README.md index 11808ef552..ff5c91fe0c 100644 --- a/selfdrive/modeld/models/README.md +++ b/selfdrive/modeld/models/README.md @@ -20,7 +20,11 @@ To view the architecture of the ONNX networks, you can use [netron](https://netr * **traffic convention** * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2 * **feature buffer** - * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 128 + * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512 +* **nav features** + * 1 * 150 +* **nav instructions** + * 1 * 256 ### Supercombo output format (Full size: XXX x float32) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc deleted file mode 100644 index 0a7f0c949d..0000000000 --- a/selfdrive/modeld/models/driving.cc +++ /dev/null @@ -1,330 +0,0 @@ -#include "selfdrive/modeld/models/driving.h" - -#include - - -void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) { - std::array lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; - const auto &best_prediction = leads.get_best_prediction(t_idx); - lead.setProb(sigmoid(leads.prob[t_idx])); - lead.setProbTime(prob_t); - std::array lead_x, lead_y, lead_v, lead_a; - std::array lead_x_std, lead_y_std, lead_v_std, lead_a_std; - for (int i=0; i desire_state_softmax; - softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN); - - std::array desire_pred_softmax; - for (int i=0; i lat_long_t = {2, 4, 6, 8, 10}; - std::array gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid, - brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid; - for (int i=0; i threshold; - } - for (int i=0; i FCW_THRESHOLD_3MS2; - } - - auto disengage = meta.initDisengagePredictions(); - disengage.setT(to_kj_array_ptr(lat_long_t)); - disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid)); - disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid)); - disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid)); - disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid)); - disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid)); - disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid)); - - meta.setEngagedProb(sigmoid(meta_data.engaged_prob)); - meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax)); - meta.setDesireState(to_kj_array_ptr(desire_state_softmax)); - meta.setHardBrakePredicted(above_fcw_threshold); -} - -void fill_confidence(cereal::ModelDataV2::Builder &framed, PublishState &ps) { - if (framed.getFrameId() % (2*MODEL_FREQ) == 0) { - // update every 2s to match predictions interval - auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs(); - auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs(); - auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs(); - - float any_dp[DISENGAGE_LEN]; - float dp_ind[DISENGAGE_LEN]; - - for (int i = 0; i < DISENGAGE_LEN; i++) { - any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob - } - - dp_ind[0] = any_dp[0]; - for (int i = 0; i < DISENGAGE_LEN-1; i++) { - dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice - } - - // rolling buf for 2, 4, 6, 8, 10s - std::memmove(&ps.disengage_buffer[0], &ps.disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1)); - std::memcpy(&ps.disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN); - } - - float score = 0; - for (int i = 0; i < DISENGAGE_LEN; i++) { - score += ps.disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN; - } - - if (score < RYG_GREEN) { - framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN); - } else if (score < RYG_YELLOW) { - framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW); - } else { - framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED); - } -} - -template -void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array &t, - const std::array &x, const std::array &y, const std::array &z) { - xyzt.setT(to_kj_array_ptr(t)); - xyzt.setX(to_kj_array_ptr(x)); - xyzt.setY(to_kj_array_ptr(y)); - xyzt.setZ(to_kj_array_ptr(z)); -} - -template -void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array &t, - const std::array &x, const std::array &y, const std::array &z, - const std::array &x_std, const std::array &y_std, const std::array &z_std) { - fill_xyzt(xyzt, t, x, y, z); - xyzt.setXStd(to_kj_array_ptr(x_std)); - xyzt.setYStd(to_kj_array_ptr(y_std)); - xyzt.setZStd(to_kj_array_ptr(z_std)); -} - -void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) { - std::array pos_x, pos_y, pos_z; - std::array pos_x_std, pos_y_std, pos_z_std; - std::array vel_x, vel_y, vel_z; - std::array rot_x, rot_y, rot_z; - std::array acc_x, acc_y, acc_z; - std::array rot_rate_x, rot_rate_y, rot_rate_z; - - for (int i=0; i &plan_t, - const ModelOutputLaneLines &lanes) { - std::array left_far_y, left_far_z; - std::array left_near_y, left_near_z; - std::array right_near_y, right_near_z; - std::array right_far_y, right_far_z; - for (int j=0; j &plan_t, - const ModelOutputRoadEdges &edges) { - std::array left_y, left_z; - std::array right_y, right_z; - for (int j=0; j plan_t; - std::fill_n(plan_t.data(), plan_t.size(), NAN); - plan_t[0] = 0.0; - for (int xidx=1, tidx=0; xidx t_offsets = {0.0, 2.0, 4.0}; - for (int i=0; i vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; - auto framed = msg.initEvent(valid).initModelV2(); - framed.setFrameId(vipc_frame_id); - framed.setFrameIdExtra(vipc_frame_id_extra); - framed.setFrameAge(frame_age); - framed.setFrameDropPerc(frame_drop * 100); - framed.setTimestampEof(timestamp_eof); - framed.setLocationMonoTime(timestamp_llk); - framed.setModelExecutionTime(model_execution_time); - framed.setNavEnabled(nav_enabled); - if (send_raw_pred) { - framed.setRawPredictions(kj::ArrayPtr(net_output_data, NET_OUTPUT_SIZE).asBytes()); - } - fill_model(framed, *((ModelOutput*) net_output_data), ps); -} - -void fill_pose_msg(MessageBuilder &msg, float *net_output_data, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid) { - const ModelOutput &net_outputs = *((ModelOutput*) net_output_data); - const auto &v_mean = net_outputs.pose.velocity_mean; - const auto &r_mean = net_outputs.pose.rotation_mean; - const auto &t_mean = net_outputs.wide_from_device_euler.mean; - const auto &v_std = net_outputs.pose.velocity_std; - const auto &r_std = net_outputs.pose.rotation_std; - const auto &t_std = net_outputs.wide_from_device_euler.std; - const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean; - const auto &road_transform_trans_std = net_outputs.road_transform.position_std; - - auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry(); - posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z}); - posenetd.setRot({r_mean.x, r_mean.y, r_mean.z}); - posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z}); - posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z}); - posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)}); - posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)}); - posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)}); - posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)}); - - posenetd.setTimestampEof(timestamp_eof); - posenetd.setFrameId(vipc_frame_id); -} diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h deleted file mode 100644 index 5df172dad6..0000000000 --- a/selfdrive/modeld/models/driving.h +++ /dev/null @@ -1,257 +0,0 @@ -#pragma once - -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/modeldata.h" -#include "common/util.h" -#include "selfdrive/modeld/models/commonmodel.h" -#include "selfdrive/modeld/runners/run.h" - -constexpr int FEATURE_LEN = 512; -constexpr int HISTORY_BUFFER_LEN = 99; -constexpr int DESIRE_LEN = 8; -constexpr int DESIRE_PRED_LEN = 4; -constexpr int TRAFFIC_CONVENTION_LEN = 2; -constexpr int NAV_FEATURE_LEN = 256; -constexpr int NAV_INSTRUCTION_LEN = 150; -constexpr int DRIVING_STYLE_LEN = 12; -constexpr int MODEL_FREQ = 20; - -constexpr int DISENGAGE_LEN = 5; -constexpr int BLINKER_LEN = 6; -constexpr int META_STRIDE = 7; - -constexpr int PLAN_MHP_N = 5; -constexpr int LEAD_MHP_N = 2; -constexpr int LEAD_TRAJ_LEN = 6; -constexpr int LEAD_MHP_SELECTION = 3; -// Padding to get output shape as multiple of 4 -constexpr int PAD_SIZE = 2; - -constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15; -constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05; -constexpr float FCW_THRESHOLD_3MS2 = 0.7; - -struct ModelOutputXYZ { - float x; - float y; - float z; -}; -static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3); - -struct ModelOutputYZ { - float y; - float z; -}; -static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2); - -struct ModelOutputPlanElement { - ModelOutputXYZ position; - ModelOutputXYZ velocity; - ModelOutputXYZ acceleration; - ModelOutputXYZ rotation; - ModelOutputXYZ rotation_rate; -}; -static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5); - -struct ModelOutputPlanPrediction { - std::array mean; - std::array std; - float prob; -}; -static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float)); - -struct ModelOutputPlans { - std::array prediction; - - constexpr const ModelOutputPlanPrediction &get_best_prediction() const { - int max_idx = 0; - for (int i = 1; i < prediction.size(); i++) { - if (prediction[i].prob > prediction[max_idx].prob) { - max_idx = i; - } - } - return prediction[max_idx]; - } -}; -static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N); - -struct ModelOutputLinesXY { - std::array left_far; - std::array left_near; - std::array right_near; - std::array right_far; -}; -static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4); - -struct ModelOutputLineProbVal { - float val_deprecated; - float val; -}; -static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2); - -struct ModelOutputLinesProb { - ModelOutputLineProbVal left_far; - ModelOutputLineProbVal left_near; - ModelOutputLineProbVal right_near; - ModelOutputLineProbVal right_far; -}; -static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4); - -struct ModelOutputLaneLines { - ModelOutputLinesXY mean; - ModelOutputLinesXY std; - ModelOutputLinesProb prob; -}; -static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb)); - -struct ModelOutputEdgessXY { - std::array left; - std::array right; -}; -static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2); - -struct ModelOutputRoadEdges { - ModelOutputEdgessXY mean; - ModelOutputEdgessXY std; -}; -static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2)); - -struct ModelOutputLeadElement { - float x; - float y; - float velocity; - float acceleration; -}; -static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4); - -struct ModelOutputLeadPrediction { - std::array mean; - std::array std; - std::array prob; -}; -static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION)); - -struct ModelOutputLeads { - std::array prediction; - std::array prob; - - constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const { - int max_idx = 0; - for (int i = 1; i < prediction.size(); i++) { - if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) { - max_idx = i; - } - } - return prediction[max_idx]; - } -}; -static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION)); - - -struct ModelOutputPose { - ModelOutputXYZ velocity_mean; - ModelOutputXYZ rotation_mean; - ModelOutputXYZ velocity_std; - ModelOutputXYZ rotation_std; -}; -static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4); - -struct ModelOutputWideFromDeviceEuler { - ModelOutputXYZ mean; - ModelOutputXYZ std; -}; -static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2); - -struct ModelOutputTemporalPose { - ModelOutputXYZ velocity_mean; - ModelOutputXYZ rotation_mean; - ModelOutputXYZ velocity_std; - ModelOutputXYZ rotation_std; -}; -static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4); - -struct ModelOutputRoadTransform { - ModelOutputXYZ position_mean; - ModelOutputXYZ rotation_mean; - ModelOutputXYZ position_std; - ModelOutputXYZ rotation_std; -}; -static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4); - -struct ModelOutputDisengageProb { - float gas_disengage; - float brake_disengage; - float steer_override; - float brake_3ms2; - float brake_4ms2; - float brake_5ms2; - float gas_pressed; -}; -static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7); - -struct ModelOutputBlinkerProb { - float left; - float right; -}; -static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2); - -struct ModelOutputDesireProb { - union { - struct { - float none; - float turn_left; - float turn_right; - float lane_change_left; - float lane_change_right; - float keep_left; - float keep_right; - float null; - }; - struct { - std::array array; - }; - }; -}; -static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN); - -struct ModelOutputMeta { - ModelOutputDesireProb desire_state_prob; - float engaged_prob; - std::array disengage_prob; - std::array blinker_prob; - std::array desire_pred_prob; -}; -static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN)); - -struct ModelOutputFeatures { - std::array feature; -}; -static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN)); - -struct ModelOutput { - const ModelOutputPlans plans; - const ModelOutputLaneLines lane_lines; - const ModelOutputRoadEdges road_edges; - const ModelOutputLeads leads; - const ModelOutputMeta meta; - const ModelOutputPose pose; - const ModelOutputWideFromDeviceEuler wide_from_device_euler; - const ModelOutputTemporalPose temporal_pose; - const ModelOutputRoadTransform road_transform; -}; - -constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); -constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE; - -struct PublishState { - std::array disengage_buffer = {}; - std::array prev_brake_5ms2_probs = {}; - std::array prev_brake_3ms2_probs = {}; -}; - -void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, - uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid); -void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid); diff --git a/selfdrive/modeld/models/driving.pxd b/selfdrive/modeld/models/driving.pxd deleted file mode 100644 index 8d6b8d755e..0000000000 --- a/selfdrive/modeld/models/driving.pxd +++ /dev/null @@ -1,25 +0,0 @@ -# distutils: language = c++ - -from libcpp cimport bool -from libc.stdint cimport uint32_t, uint64_t - -cdef extern from "cereal/messaging/messaging.h": - cdef cppclass MessageBuilder: - size_t getSerializedSize() - int serializeToBuffer(unsigned char *, size_t) - -cdef extern from "selfdrive/modeld/models/driving.h": - cdef int FEATURE_LEN - cdef int HISTORY_BUFFER_LEN - cdef int DESIRE_LEN - cdef int TRAFFIC_CONVENTION_LEN - cdef int DRIVING_STYLE_LEN - cdef int NAV_FEATURE_LEN - cdef int NAV_INSTRUCTION_LEN - cdef int OUTPUT_SIZE - cdef int NET_OUTPUT_SIZE - cdef int MODEL_FREQ - cdef struct PublishState: pass - - void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool) - void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool) diff --git a/selfdrive/modeld/models/driving_pyx.pyx b/selfdrive/modeld/models/driving_pyx.pyx deleted file mode 100644 index b98a8f3ff1..0000000000 --- a/selfdrive/modeld/models/driving_pyx.pyx +++ /dev/null @@ -1,52 +0,0 @@ -# distutils: language = c++ -# cython: c_string_encoding=ascii - -import numpy as np -cimport numpy as cnp -from libcpp cimport bool -from libc.string cimport memcpy -from libc.stdint cimport uint32_t, uint64_t - -from .commonmodel cimport mat3 -from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \ - TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \ - NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \ - OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ -from .driving cimport MessageBuilder, PublishState as cppPublishState -from .driving cimport fill_model_msg, fill_pose_msg - -FEATURE_LEN = CPP_FEATURE_LEN -HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN -DESIRE_LEN = CPP_DESIRE_LEN -TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN -DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN -NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN -NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN -OUTPUT_SIZE = CPP_OUTPUT_SIZE -NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE -MODEL_FREQ = CPP_MODEL_FREQ - -cdef class PublishState: - cdef cppPublishState state - -def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop, - uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid): - cdef MessageBuilder msg - fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop, - timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid) - - output_size = msg.getSerializedSize() - output_data = bytearray(output_size) - cdef unsigned char * output_ptr = output_data - assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" - return bytes(output_data) - -def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid): - cdef MessageBuilder msg - fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid) - - output_size = msg.getSerializedSize() - output_data = bytearray(output_size) - cdef unsigned char * output_ptr = output_data - assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize" - return bytes(output_data) diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py index f5dc51ed20..90b9762800 100755 --- a/selfdrive/modeld/navmodeld.py +++ b/selfdrive/modeld/navmodeld.py @@ -13,13 +13,13 @@ from cereal.visionipc import VisionIpcClient, VisionStreamType from openpilot.system.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority -from openpilot.selfdrive.modeld.constants import IDX_N +from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime NAV_INPUT_SIZE = 256*256 NAV_FEATURE_LEN = 256 NAV_DESIRE_LEN = 32 -NAV_OUTPUT_SIZE = 2*2*IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN +NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN MODEL_PATHS = { ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc', ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'} @@ -31,8 +31,8 @@ class NavModelOutputXY(ctypes.Structure): class NavModelOutputPlan(ctypes.Structure): _fields_ = [ - ("mean", NavModelOutputXY*IDX_N), - ("std", NavModelOutputXY*IDX_N)] + ("mean", NavModelOutputXY*ModelConstants.IDX_N), + ("std", NavModelOutputXY*ModelConstants.IDX_N)] class NavModelResult(ctypes.Structure): _fields_ = [ diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py new file mode 100644 index 0000000000..a7b160fcea --- /dev/null +++ b/selfdrive/modeld/parse_model_outputs.py @@ -0,0 +1,100 @@ +import numpy as np +from typing import Dict +from openpilot.selfdrive.modeld.constants import ModelConstants + +def sigmoid(x): + return 1. / (1. + np.exp(-x)) + +def softmax(x, axis=-1): + x -= np.max(x, axis=axis, keepdims=True) + if x.dtype == np.float32 or x.dtype == np.float64: + np.exp(x, out=x) + else: + x = np.exp(x) + x /= np.sum(x, axis=axis, keepdims=True) + return x + +class Parser: + def __init__(self, ignore_missing=False): + self.ignore_missing = ignore_missing + + def check_missing(self, outs, name): + if name not in outs and not self.ignore_missing: + raise ValueError(f"Missing output {name}") + return name not in outs + + def parse_categorical_crossentropy(self, name, outs, out_shape=None): + if self.check_missing(outs, name): + return + raw = outs[name] + if out_shape is not None: + raw = raw.reshape((raw.shape[0],) + out_shape) + outs[name] = softmax(raw, axis=-1) + + def parse_binary_crossentropy(self, name, outs): + if self.check_missing(outs, name): + return + raw = outs[name] + outs[name] = sigmoid(raw) + + def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None): + if self.check_missing(outs, name): + return + raw = outs[name] + raw = raw.reshape((raw.shape[0], max(in_N, 1), -1)) + + pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2] + n_values = (raw.shape[2] - out_N)//2 + pred_mu = raw[:,:,:n_values] + pred_std = np.exp(raw[:,:,n_values: 2*n_values]) + + if in_N > 1: + weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype) + for i in range(out_N): + weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1) + + if out_N == 1: + for fidx in range(weights.shape[0]): + idxs = np.argsort(weights[fidx][:,0])[::-1] + weights[fidx] = weights[fidx][idxs] + pred_mu[fidx] = pred_mu[fidx][idxs] + pred_std[fidx] = pred_std[fidx][idxs] + full_shape = tuple([raw.shape[0], in_N] + list(out_shape)) + outs[name + '_weights'] = weights + outs[name + '_hypotheses'] = pred_mu.reshape(full_shape) + outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape) + + pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) + pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype) + for fidx in range(weights.shape[0]): + for hidx in range(out_N): + idxs = np.argsort(weights[fidx,:,hidx])[::-1] + pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]] + pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]] + else: + pred_mu_final = pred_mu + pred_std_final = pred_std + + if out_N > 1: + final_shape = tuple([raw.shape[0], out_N] + list(out_shape)) + else: + final_shape = tuple([raw.shape[0],] + list(out_shape)) + outs[name] = pred_mu_final.reshape(final_shape) + outs[name + '_stds'] = pred_std_final.reshape(final_shape) + + def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]: + self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION, + out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH)) + self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) + 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,)) + 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)) + 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,)) + self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH)) + return outs diff --git a/selfdrive/modeld/thneed/lib.py b/selfdrive/modeld/thneed/lib.py deleted file mode 100644 index c058638fad..0000000000 --- a/selfdrive/modeld/thneed/lib.py +++ /dev/null @@ -1,32 +0,0 @@ -import struct -import json - -def load_thneed(fn): - with open(fn, "rb") as f: - json_len = struct.unpack("I", f.read(4))[0] - jdat = json.loads(f.read(json_len).decode('latin_1')) - weights = f.read() - ptr = 0 - for o in jdat['objects']: - if o['needs_load']: - nptr = ptr + o['size'] - o['data'] = weights[ptr:nptr] - ptr = nptr - for o in jdat['binaries']: - nptr = ptr + o['length'] - o['data'] = weights[ptr:nptr] - ptr = nptr - return jdat - -def save_thneed(jdat, fn): - new_weights = [] - for o in jdat['objects'] + jdat['binaries']: - if 'data' in o: - new_weights.append(o['data']) - del o['data'] - new_weights_bytes = b''.join(new_weights) - with open(fn, "wb") as f: - j = json.dumps(jdat, ensure_ascii=False).encode('latin_1') - f.write(struct.pack("I", len(j))) - f.write(j) - f.write(new_weights_bytes) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 0dce1a0f9b..bb935fdc8e 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -6,7 +6,7 @@ from cereal import log import cereal.messaging as messaging from openpilot.common.realtime import Ratekeeper, DT_MDL from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.modeld.constants import T_IDXS +from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU @@ -100,13 +100,13 @@ class Plant: # this is to ensure lead policy is effective when model # does not predict slowdown in e2e mode position = log.XYZTData.new_message() - position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)] + position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] model.modelV2.position = position velocity = log.XYZTData.new_message() - velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)] + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] model.modelV2.velocity = velocity acceleration = log.XYZTData.new_message() - acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)] + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 626af12661..2a4113a77f 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -f851c7e7f90eff828a59444d20fac5df8cd7ae0c +0e0f55cf3bb2cf79b44adf190e6387a83deb6646 diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 5c731bc303..4a9fc85d2e 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -37,7 +37,7 @@ PROCS = { "selfdrive.locationd.paramsd": 9.0, "./sensord": 7.0, "selfdrive.controls.radard": 4.5, - "selfdrive.modeld.modeld": 8.0, + "selfdrive.modeld.modeld": 13.0, "selfdrive.modeld.dmonitoringmodeld": 8.0, "selfdrive.modeld.navmodeld": 1.0, "selfdrive.thermald.thermald": 3.87, diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py index 1409e43cfb..1ec9b6ec59 100755 --- a/system/hardware/tici/tests/test_power_draw.py +++ b/system/hardware/tici/tests/test_power_draw.py @@ -28,7 +28,7 @@ class Proc: PROCS = [ Proc('camerad', 2.1, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), - Proc('modeld', 0.93, atol=0.2, msgs=['modelV2']), + Proc('modeld', 1.0, atol=0.2, msgs=['modelV2']), Proc('dmonitoringmodeld', 0.4, msgs=['driverStateV2']), Proc('encoderd', 0.23, msgs=[]), Proc('mapsd', 0.05, msgs=['mapRenderState']), diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index 9fd8a56101..7dffa7b5e5 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -31,6 +31,7 @@ COPY ./panda ${OPENPILOT_PATH}/panda COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive COPY ./system ${OPENPILOT_PATH}/system COPY ./tools ${OPENPILOT_PATH}/tools +COPY ./release ${OPENPILOT_PATH}/release RUN --mount=type=bind,source=.ci_cache/scons_cache,target=/tmp/scons_cache,rw scons -j$(nproc) --cache-readonly