|
|
|
@@ -1,5 +1,7 @@
|
|
|
|
|
import numpy as np
|
|
|
|
|
from openpilot.common.params import Params
|
|
|
|
|
from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
|
|
|
|
|
from openpilot.sunnypilot.models.helpers import get_active_bundle
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def safe_exp(x, out=None):
|
|
|
|
@@ -24,6 +26,9 @@ def softmax(x, axis=-1):
|
|
|
|
|
class Parser:
|
|
|
|
|
def __init__(self, ignore_missing=False):
|
|
|
|
|
self.ignore_missing = ignore_missing
|
|
|
|
|
self._params = Params()
|
|
|
|
|
model_bundle = get_active_bundle()
|
|
|
|
|
self.generation = model_bundle.generation if model_bundle is not None else None
|
|
|
|
|
|
|
|
|
|
def check_missing(self, outs, name):
|
|
|
|
|
if name not in outs and not self.ignore_missing:
|
|
|
|
@@ -88,21 +93,31 @@ class Parser:
|
|
|
|
|
outs[name] = pred_mu_final.reshape(final_shape)
|
|
|
|
|
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
|
|
|
|
|
|
|
|
|
def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
|
|
|
|
|
if 'lead' in outs:
|
|
|
|
|
if outs['lead'].shape[1] == 2 * SplitModelConstants.LEAD_MHP_SELECTION *SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH:
|
|
|
|
|
self.parse_mdn('lead', outs, in_N=0, out_N=0,
|
|
|
|
|
out_shape=(SplitModelConstants.LEAD_MHP_SELECTION, SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
|
|
|
|
else:
|
|
|
|
|
def parse_dynamic_outputs(self, outs: dict[str, np.ndarray]) -> None:
|
|
|
|
|
if self._params.get_bool("DynamicModeldOutputs") or (self.generation >= 12):
|
|
|
|
|
if 'lead' in outs:
|
|
|
|
|
if outs['lead'].shape[1] == 2 * SplitModelConstants.LEAD_MHP_SELECTION *SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH:
|
|
|
|
|
self.parse_mdn('lead', outs, in_N=0, out_N=0,
|
|
|
|
|
out_shape=(SplitModelConstants.LEAD_MHP_SELECTION, SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
|
|
|
|
else:
|
|
|
|
|
self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION,
|
|
|
|
|
out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
|
|
|
|
if 'plan' in outs:
|
|
|
|
|
if outs['plan'].shape[1] > 2 * SplitModelConstants.PLAN_WIDTH * SplitModelConstants.IDX_N:
|
|
|
|
|
self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION,
|
|
|
|
|
out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
|
|
|
|
|
else:
|
|
|
|
|
self.parse_mdn('plan', outs, in_N=0, out_N=0,
|
|
|
|
|
out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
|
|
|
|
|
else:
|
|
|
|
|
if 'lead' in outs:
|
|
|
|
|
self.parse_mdn('lead', outs, in_N=SplitModelConstants.LEAD_MHP_N, out_N=SplitModelConstants.LEAD_MHP_SELECTION,
|
|
|
|
|
out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
|
|
|
|
if 'plan' in outs:
|
|
|
|
|
if outs['plan'].shape[1] > 2 * SplitModelConstants.PLAN_WIDTH * SplitModelConstants.IDX_N:
|
|
|
|
|
out_shape=(SplitModelConstants.LEAD_TRAJ_LEN,SplitModelConstants.LEAD_WIDTH))
|
|
|
|
|
if 'plan' in outs:
|
|
|
|
|
self.parse_mdn('plan', outs, in_N=SplitModelConstants.PLAN_MHP_N, out_N=SplitModelConstants.PLAN_MHP_SELECTION,
|
|
|
|
|
out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
|
|
|
|
|
else:
|
|
|
|
|
self.parse_mdn('plan', outs, in_N=0, out_N=0,
|
|
|
|
|
out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.PLAN_WIDTH))
|
|
|
|
|
|
|
|
|
|
def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
|
|
|
|
|
if 'lane_lines' in outs:
|
|
|
|
|
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
|
|
|
|
|
out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
|
|
|
|
@@ -117,12 +132,14 @@ class Parser:
|
|
|
|
|
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
|
|
|
|
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
|
|
|
|
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
|
|
|
|
|
self.parse_dynamic_outputs(outs)
|
|
|
|
|
self.split_outputs(outs)
|
|
|
|
|
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
|
|
|
|
|
self.parse_binary_crossentropy('meta', outs)
|
|
|
|
|
return outs
|
|
|
|
|
|
|
|
|
|
def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
|
|
|
|
|
self.parse_dynamic_outputs(outs)
|
|
|
|
|
self.split_outputs(outs)
|
|
|
|
|
if 'lat_planner_solution' in outs:
|
|
|
|
|
self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
|
|
|
|
|