diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 115ec94f88fcbd..40c9586383562c 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -8,6 +8,15 @@ ConfidenceClass = log.ModelDataV2.ConfidenceClass +def curv_from_psis(psi_target, psi_rate, vego, delay): + curv_from_psi = psi_target / ((vego + 1e-3) * delay) # epsilon to prevent divide-by-zero + return 2*curv_from_psi - psi_rate / (1e-3 + vego) + +def get_curvature_from_plan(plan, vego, delay): + psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]) + psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2] + return curv_from_psis(psi_target, psi_rate, vego, delay) + class PublishState: def __init__(self): self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32) @@ -55,14 +64,17 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs): builder.rightProb = lane_line_probs[2] def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_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, model_execution_time: float, valid: bool) -> None: + net_output_data: dict[str, np.ndarray], v_ego: float, delay: float, + publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, + frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float, + valid: bool) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 frame_drop_perc = frame_drop * 100 extended_msg.valid = valid base_msg.valid = valid + desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay)) + driving_model_data = base_msg.drivingModelData driving_model_data.frameId = vipc_frame_id @@ -71,7 +83,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D driving_model_data.modelExecutionTime = model_execution_time action = driving_model_data.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = desired_curv modelV2 = extended_msg.modelV2 modelV2.frameId = vipc_frame_id @@ -106,7 +118,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # lateral planning action = modelV2.action - action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) + action.desiredCurvature = desired_curv # times at X_IDXS according to model plan PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 4e91d324007943..c160f9888c07de 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -57,14 +57,11 @@ def __init__(self, context: CLContext): self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32) self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32) self.desire_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32) - self.prev_desired_curv_20hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32) # img buffers are managed in openCL transform code self.inputs = { '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), - 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), - 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } @@ -100,7 +97,6 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_ self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten() self.inputs['traffic_convention'][:] = inputs['traffic_convention'] - self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))) @@ -114,13 +110,8 @@ def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_ self.full_features_20Hz[:-1] = self.full_features_20Hz[1:] self.full_features_20Hz[-1] = outputs['hidden_state'][0, :] - self.prev_desired_curv_20hz[:-1] = self.prev_desired_curv_20hz[1:] - self.prev_desired_curv_20hz[-1] = outputs['desired_curvature'][0, :] - idxs = np.arange(-4,-100,-4)[::-1] self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten() - # TODO model only uses last value now, once that changes we need to input strided action history buffer - self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = 0. * self.prev_desired_curv_20hz[-4, :] return outputs @@ -231,7 +222,6 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - lateral_control_params = np.array([v_ego, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -262,7 +252,6 @@ def main(demo=False): inputs:dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, - 'lateral_control_params': lateral_control_params, } mt1 = time.perf_counter() @@ -274,7 +263,8 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - fill_model_msg(drivingdata_send, modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, + fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay, + publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen) desire_state = modelv2_send.modelV2.meta.desireState diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 5ec978e56636b7..dce3ad41d5894c 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2a845fd16d6482222c574db833d2badb37ebcdf9c7d2987ab347ef63e728a146 -size 50309976 +oid sha256:58d6a6f77438addd7666b6b3257f78b369e4ef504434522c185e4c671187fb67 +size 49095720