diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 961b8069d4..6a5b22f686 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,8 +3,7 @@ import math 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, DT_CTRL -from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable @@ -22,7 +21,6 @@ CAR_ROTATION_RADIUS = 0.0 # EU guidelines MAX_LATERAL_JERK = 5.0 - MAX_VEL_ERR = 5.0 ButtonEvent = car.CarState.ButtonEvent @@ -169,28 +167,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): safe_desired_curvature = clip(new_curvature, prev_curvature - max_curvature_rate * DT_CTRL, prev_curvature + max_curvature_rate * DT_CTRL) - return safe_desired_curvature - - -def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures): - if len(psis) != CONTROL_N: - psis = [0.0]*CONTROL_N - curvatures = [0.0]*CONTROL_N - v_ego = max(MIN_SPEED, v_ego) - - # MPC can plan to turn the wheel and turn back before t_delay. This means - # 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(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis) - average_curvature_desired = psi / (v_ego * steer_delay) - desired_curvature = 2 * average_curvature_desired - current_curvature_desired - - # This is the "desired rate of the setpoint" not an actual desired rate - max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - safe_desired_curvature = clip(desired_curvature, - current_curvature_desired - max_curvature_rate * DT_MDL, - current_curvature_desired + max_curvature_rate * DT_MDL) return safe_desired_curvature diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index 4d3af51635..e513922c72 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -21,7 +21,8 @@ class ModelConstants: NAV_FEATURE_LEN = 256 NAV_INSTRUCTION_LEN = 150 DRIVING_STYLE_LEN = 12 - LAT_PLANNER_STATE_LEN = 4 + LATERAL_CONTROL_PARAMS_LEN = 2 + PREV_DESIRED_CURVS_LEN = 20 # model outputs constants FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) @@ -38,7 +39,7 @@ class ModelConstants: ROAD_EDGES_WIDTH = 2 PLAN_WIDTH = 15 DESIRE_PRED_WIDTH = 8 - LAT_PLANNER_SOLUTION_WIDTH = 4 + DESIRED_CURV_WIDTH = 1 NUM_LANE_LINES = 4 NUM_ROAD_EDGES = 2 diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 4a47571be7..93d1c7e77b 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -3,7 +3,6 @@ import capnp import numpy as np from typing import Dict from cereal import log -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -73,14 +72,8 @@ 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 - x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)] - x_sol = np.column_stack([x, y, yaw, yawRate]) - v_ego = max(MIN_SPEED, v_ego) - psis = x_sol[0:CONTROL_N, 2].tolist() - curvatures = (x_sol[0:CONTROL_N, 3]/v_ego).tolist() - action = modelV2.action - action.desiredCurvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures) + action.desiredCurvature = float(net_output_data['desired_curvature'][0,0]) # 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 0b57228a7a..00c46c3bce 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -12,8 +12,6 @@ from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL -from openpilot.common.numpy_fast import interp from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process from openpilot.common.transformations.model import get_warp_matrix @@ -59,7 +57,8 @@ class ModelState: 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), - 'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32), + '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), '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), @@ -94,6 +93,7 @@ class ModelState: self.prev_desire[:] = inputs['desire'] self.inputs['traffic_convention'][:] = inputs['traffic_convention'] + self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] self.inputs['nav_features'][:] = inputs['nav_features'] self.inputs['nav_instructions'][:] = inputs['nav_instructions'] @@ -110,8 +110,8 @@ 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, :] - 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]) + self.inputs['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:] + self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0] return outputs @@ -152,8 +152,12 @@ def main(demo=False): pm = PubMaster(["modelV2", "cameraOdometry"]) sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) + publish_state = PublishState() params = Params() + with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg: + steer_delay = msg.steerActuatorDelay + .2 + #steer_delay = 0.4 # setup filter to track dropped frames frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ) @@ -221,6 +225,8 @@ def main(demo=False): v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId + # TODO add lag + lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"]: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32) @@ -274,6 +280,7 @@ def main(demo=False): inputs:Dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, + 'lateral_control_params': lateral_control_params, 'nav_features': nav_features, 'nav_instructions': nav_instructions} diff --git a/selfdrive/modeld/models/navmodel.onnx b/selfdrive/modeld/models/navmodel.onnx index 93edb593cc..3b687b960a 100644 --- a/selfdrive/modeld/models/navmodel.onnx +++ b/selfdrive/modeld/models/navmodel.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4971931accb5ba2e534bb3e0c591826ee507e2988df2eccf1fe862c303ddf9c5 -size 14221074 +oid sha256:8254b569878b7472e3f63ed9f3527a87bde785c9037aee3ed66f972e072b5899 +size 14166696 diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc index cb6e55b75f..d5e43abcfb 100644 --- a/selfdrive/modeld/models/navmodel_q.dlc +++ b/selfdrive/modeld/models/navmodel_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:fa346ada6f8c6326a5ee5fcd27e45e3e710049358079413c6a4624b20c6e1e47 +oid sha256:89fda8380efa3e421fbcdb6bb204c36a4991f137ee01d47f3d0380895aa7c036 size 3630942 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index ca8642093d..e91940c728 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:ae44fe832fe48b89998f09cebb1bcd129864a8f51497b636cd38e66e46d69a89 -size 48457850 +oid sha256:4b085c1ba231bc381f78462bda136172787371d5d83b6e1bcd340aad17290ebc +size 48197170 diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 9d37a5fad4..07d5e0a921 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -93,7 +93,7 @@ class Parser: 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)) - self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH)) + self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,)) for k in ['lead_prob', 'lane_lines_prob', 'meta']: self.parse_binary_crossentropy(k, outs) self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 3f65f36e18..85bcf123e7 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -0513d29764980f512710cc2ebd7c14f91ae0351d +cfdad3a695e3562ca32accce043b358291f0eef2