From d5e1e462d770abcc7b623dcc8a649203543ac30c Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 31 Oct 2024 11:29:30 -0700 Subject: [PATCH 1/9] 92e0c05d-936d-4528-9962-2e9cf51645b1/270 --- selfdrive/modeld/models/supercombo.onnx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 5ec978e56636b7..cf6210b3f58f49 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:7b4be3272a1621e087b4c4e440f3b24944bbf61ea87941df8616175a13a9b37a +size 60659298 From ca9d8225b9d417f34d57319ac233a447372cc272 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 31 Oct 2024 11:46:15 -0700 Subject: [PATCH 2/9] Just index into plan --- selfdrive/modeld/fill_model_msg.py | 22 +++++++++++++++++----- selfdrive/modeld/modeld.py | 14 ++------------ 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 115ec94f88fcbd..55cea129323b88 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 From b63edb5c62925c11655f92146208ade7d1e3a332 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 31 Oct 2024 11:54:27 -0700 Subject: [PATCH 3/9] whitespace --- selfdrive/modeld/fill_model_msg.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index 55cea129323b88..40c9586383562c 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -64,7 +64,7 @@ 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], v_ego: float, delay: float, + 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: From 83476c501d426f4817cc8fd4f2c35e6f2647531a Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Thu, 31 Oct 2024 16:28:34 -0700 Subject: [PATCH 4/9] 92e0c05d-936d-4528-9962-2e9cf51645b1/400 --- selfdrive/modeld/models/supercombo.onnx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index cf6210b3f58f49..df47bf67e9ca3e 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:7b4be3272a1621e087b4c4e440f3b24944bbf61ea87941df8616175a13a9b37a +oid sha256:df504352402b6a1701c449f6955ab0e00aba95516fcb7c809d4a318d2846d91e size 60659298 From 710f85099cbd5ce2800e82f93c9e37311ed9aa7f Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 1 Nov 2024 16:03:11 -0700 Subject: [PATCH 5/9] aeed0f97-7827-4188-8c6f-a7c07c0e0b9a/400 --- selfdrive/modeld/models/supercombo.onnx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index df47bf67e9ca3e..471b6131f5ca1b 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:df504352402b6a1701c449f6955ab0e00aba95516fcb7c809d4a318d2846d91e -size 60659298 +oid sha256:45bb2154d93d729d057de201aad923c033e74f892acbecfc86661ad99b640566 +size 60659296 From 50c4ddcf4aeaf8f4f536d38ab0dbd3754b3b453c Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Fri, 1 Nov 2024 16:26:28 -0700 Subject: [PATCH 6/9] aeed0f97-7827-4188-8c6f-a7c07c0e0b9a/400 --- selfdrive/modeld/models/supercombo.onnx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 471b6131f5ca1b..b8859efec10f8e 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:45bb2154d93d729d057de201aad923c033e74f892acbecfc86661ad99b640566 -size 60659296 +oid sha256:45099f06a8d3792c0182fbf9160d5d93d21238091c6adc184acd970ccdfe215d +size 49095720 From dbf9b94afe02d694ba773296925257807a53949a Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Sat, 2 Nov 2024 10:10:04 -0700 Subject: [PATCH 7/9] f2a668f5-b74c-41dd-b0bc-1e2f7fcad4fa/40 --- selfdrive/modeld/models/supercombo.onnx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index b8859efec10f8e..ce516333da0c9e 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:45099f06a8d3792c0182fbf9160d5d93d21238091c6adc184acd970ccdfe215d -size 49095720 +oid sha256:59f3cdd62f81ab4b0e824037ced79ddd68f2cb6b4dfb90b5c62266aa8d39a487 +size 49095719 From 7b013da161fe0143e85b8e20e090f12b19844585 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Sat, 2 Nov 2024 17:30:49 -0700 Subject: [PATCH 8/9] ba29971c-ed5d-4cb1-abdc-a9cd3507ea52/100 --- selfdrive/modeld/models/supercombo.onnx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index ce516333da0c9e..6a7f9e3070705f 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:59f3cdd62f81ab4b0e824037ced79ddd68f2cb6b4dfb90b5c62266aa8d39a487 -size 49095719 +oid sha256:232d7f7d7d9f57137fe25e40a21f1db9b8d059fca3443f7b8708b8a5711558d4 +size 49095720 From 838022238572d4680ae6d8294dd36ddcf36a8bfa Mon Sep 17 00:00:00 2001 From: Yassine Date: Sun, 3 Nov 2024 10:54:15 -0800 Subject: [PATCH 9/9] ab82542d-00ba-4820-a285-953bf6c39dc7/400 --- selfdrive/modeld/models/supercombo.onnx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 6a7f9e3070705f..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:232d7f7d7d9f57137fe25e40a21f1db9b8d059fca3443f7b8708b8a5711558d4 +oid sha256:58d6a6f77438addd7666b6b3257f78b369e4ef504434522c185e4c671187fb67 size 49095720