mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 08:16:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

process_replay: drivingModelData migration (#33686)

Browse Source 
* Migrate modelV2 to drivingModelData

* Add if statements

* Fix
This commit is contained in:
Kacper Rączy
2024-09-30 20:42:51 -07:00
committed by GitHub
parent b0e4d6ee7a
commit 6c227a66a2
2 changed files with 32 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/fill_model_msg.py
+7 -4
View File
@@ -48,6 +48,12 @@ def fill_xyz_poly(builder, degree, x, y, z):
builder.yCoefficients = coeffs[:, 1].tolist()
builder.zCoefficients = coeffs[:, 2].tolist()
def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
builder.leftY = lane_lines[1].y[0]
builder.leftProb = lane_line_probs[1]
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], publish_state: PublishState,
vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
@@ -130,10 +136,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
lane_line_meta = driving_model_data.laneLineMeta
lane_line_meta.leftY = modelV2.laneLines[1].y[0]
lane_line_meta.leftProb = modelV2.laneLineProbs[1]
lane_line_meta.rightY = modelV2.laneLines[2].y[0]
lane_line_meta.rightProb = modelV2.laneLineProbs[2]
fill_lane_line_meta(lane_line_meta, modelV2.laneLines, modelV2.laneLineProbs)
# road edges
modelV2.init('roadEdges', 2)
selfdrive/test/process_replay/migration.py
+25
View File
@@ -3,6 +3,8 @@ from collections import defaultdict
from cereal import messaging, car
from opendbc.car.fingerprints import MIGRATION
from opendbc.car.toyota.values import EPS_SCALE
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index
from openpilot.system.manager.process_config import managed_processes
from panda import Panda
@@ -19,6 +21,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
msgs = migrate_liveLocationKalman(msgs)
msgs = migrate_liveTracks(msgs)
msgs = migrate_driverAssistance(msgs)
msgs = migrate_drivingModelData(msgs)
if manager_states:
msgs = migrate_managerState(msgs)
if panda_states:
@@ -29,6 +32,7 @@ def migrate_all(lr, manager_states=False, panda_states=False, camera_states=Fals
return msgs
def migrate_driverAssistance(lr):
all_msgs = []
for msg in lr:
@@ -39,6 +43,27 @@ def migrate_driverAssistance(lr):
return lr
return all_msgs
def migrate_drivingModelData(lr):
all_msgs = []
for msg in lr:
all_msgs.append(msg)
if msg.which() == "modelV2":
dmd = messaging.new_message('drivingModelData', valid=msg.valid, logMonoTime=msg.logMonoTime)
for field in ["frameId", "frameIdExtra", "frameDropPerc", "modelExecutionTime", "action"]:
setattr(dmd.drivingModelData, field, getattr(msg.modelV2, field))
for meta_field in ["laneChangeState", "laneChangeState"]:
setattr(dmd.drivingModelData.meta, meta_field, getattr(msg.modelV2.meta, meta_field))
if len(msg.modelV2.laneLines) and len(msg.modelV2.laneLineProbs):
fill_lane_line_meta(dmd.drivingModelData.laneLineMeta, msg.modelV2.laneLines, msg.modelV2.laneLineProbs)
if all(len(a) for a in [msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z]):
fill_xyz_poly(dmd.drivingModelData.path, ModelConstants.POLY_PATH_DEGREE, msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z)
all_msgs.append(dmd.as_reader())
elif msg.which() == "drivingModelData":
return lr
return all_msgs
def migrate_liveTracks(lr):
all_msgs = []
for msg in lr: