sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

simplify plan parsing

Browse Source
pull/30273/head
Yassine 2 years ago
parent 2c278490d8
commit 90fb43b739
1 changed files with 8 additions and 9 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      selfdrive/modeld/fill_model_msg.py

17
selfdrive/modeld/fill_model_msg.py
Unescape View File

@ -2,7 +2,7 @@ import capnp
import numpy as np import numpy as np
from typing import List, Dict from typing import List, Dict
from openpilot.selfdrive.modeld.models.driving_pyx import PublishState from openpilot.selfdrive.modeld.models.driving_pyx import PublishState
from openpilot.selfdrive.modeld.constants import T_IDXS, X_IDXS, LEAD_T_IDXS, META_T_IDXS, LEAD_T_OFFSETS, Meta from openpilot.selfdrive.modeld.constants import T_IDXS, X_IDXS, LEAD_T_IDXS, META_T_IDXS, LEAD_T_OFFSETS, Meta, Plan
# TODO: use Enum Slices when possible instead of hardcoded indices # TODO: use Enum Slices when possible instead of hardcoded indices
@ -45,16 +45,15 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
# plan # plan
position = modelV2.position position = modelV2.position
fill_xyzt(position, T_IDXS, net_output_data['plan'][0,:,0], net_output_data['plan'][0,:,1], net_output_data['plan'][0,:,2], fill_xyzt(position, T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
net_output_data['plan_stds'][0,:,0], net_output_data['plan_stds'][0,:,1], net_output_data['plan_stds'][0,:,2])
velocity = modelV2.velocity velocity = modelV2.velocity
fill_xyzt(velocity, T_IDXS, net_output_data['plan'][0,:,3], net_output_data['plan'][0,:,4], net_output_data['plan'][0,:,5]) fill_xyzt(velocity, T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
acceleration = modelV2.acceleration acceleration = modelV2.acceleration
fill_xyzt(acceleration, T_IDXS, net_output_data['plan'][0,:,6], net_output_data['plan'][0,:,7], net_output_data['plan'][0,:,8]) fill_xyzt(acceleration, T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
orientation = modelV2.orientation orientation = modelV2.orientation
fill_xyzt(orientation, T_IDXS, net_output_data['plan'][0,:,9], net_output_data['plan'][0,:,10], net_output_data['plan'][0,:,11]) fill_xyzt(orientation, T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
orientation_rate = modelV2.orientationRate orientation_rate = modelV2.orientationRate
fill_xyzt(orientation_rate, T_IDXS, net_output_data['plan'][0,:,12], net_output_data['plan'][0,:,13], net_output_data['plan'][0,:,14]) fill_xyzt(orientation_rate, T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
# lane lines # lane lines
modelV2.init('laneLines', 4) modelV2.init('laneLines', 4)
@ -86,8 +85,8 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
# meta # meta
meta = modelV2.meta meta = modelV2.meta
meta.desireState = net_output_data['desire_state'][0,:].reshape(-1).tolist() meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
meta.desirePrediction = net_output_data['desire_pred'][0,:].reshape(-1).tolist() meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item() meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
meta.init('disengagePredictions') meta.init('disengagePredictions')
disengage_predictions = meta.disengagePredictions disengage_predictions = meta.disengagePredictions

Write Preview
Loading…
Cancel
Save