mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-27 18:13:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

modeld: compress modelV2 into drivingModelData (#38165)

Browse Source 
* modeld: compress modelV2 into drivingModelData

* slightly better

* less diff

* whitespace

* dedupe with migrate?

* Revert "dedupe with migrate?"

This reverts commit f31fe34994f4331f45e14a035df4847905d37540.
This commit is contained in:
Armand du Parc Locmaria
2026-06-10 17:06:09 -07:00
committed by GitHub
parent 433c52f623
commit bdac9efa1e
2 changed files with 20 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/fill_model_msg.py
+17 -19
View File
@@ -55,26 +55,29 @@ def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
builder.rightY = lane_lines[2].y[0]
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], action: log.ModelDataV2.Action,
def fill_driving_model_data(msg: capnp._DynamicStructBuilder, modelv2_send: capnp._DynamicStructBuilder) -> None:
msg.valid = modelv2_send.valid
modelV2 = modelv2_send.modelV2
driving_model_data = msg.drivingModelData
driving_model_data.frameId = modelV2.frameId
driving_model_data.frameIdExtra = modelV2.frameIdExtra
driving_model_data.frameDropPerc = modelV2.frameDropPerc
driving_model_data.modelExecutionTime = modelV2.modelExecutionTime
driving_model_data.action = modelV2.action
driving_model_data.meta.laneChangeState = modelV2.meta.laneChangeState
driving_model_data.meta.laneChangeDirection = modelV2.meta.laneChangeDirection
fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, modelV2.position.x, modelV2.position.y, modelV2.position.z)
def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
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
msg.valid = valid
driving_model_data = base_msg.drivingModelData
driving_model_data.frameId = vipc_frame_id
driving_model_data.frameIdExtra = vipc_frame_id_extra
driving_model_data.frameDropPerc = frame_drop_perc
driving_model_data.modelExecutionTime = model_execution_time
driving_model_data.action = action
modelV2 = extended_msg.modelV2
modelV2 = msg.modelV2
modelV2.frameId = vipc_frame_id
modelV2.frameIdExtra = vipc_frame_id_extra
modelV2.frameAge = frame_age
@@ -89,9 +92,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# poly path
fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
# action
modelV2.action = action
@@ -106,8 +106,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
# road edges
modelV2.init('roadEdges', 2)
for i in range(2):
selfdrive/modeld/modeld.py
+3 -4
View File
@@ -21,7 +21,7 @@ from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState
from openpilot.common.file_chunker import read_file_chunked, get_manifest_path
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
from openpilot.selfdrive.modeld.helpers import usbgpu_present, modeld_pkl_path, get_tg_input_devices
@@ -311,7 +311,7 @@ def main(demo=False):
action = get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego)
prev_action = action
fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
fill_model_msg(modelv2_send, model_output, action,
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)
@@ -322,9 +322,8 @@ def main(demo=False):
DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
fill_driving_model_data(drivingdata_send, modelv2_send)
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('drivingModelData', drivingdata_send)