mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-11 04:12:07 +08:00
modeld: parsing and publishing in python (#30273)
* WIP try modeld all in python * fix plan * add lane lines stds * fix lane lines prob * add lead prob * add meta * simplify plan parsing * add hard brake pred * add confidence * fix desire state and desire pred * check this file for now * rm prints * rm debug * add todos * add plan_t_idxs * same as cpp * removed cython * add wfd width - rm cpp code * add new files rm old files * get metadata at compile time * forgot this file * now uses more CPU * not used * update readme * lint * copy this too * simplify disengage probs * update model replay ref commit * update again * confidence: remove if statemens * use publish_state.enqueue * Revert "use publish_state.enqueue" This reverts commit d8807c8348338a1f773a8de00fd796abb8181404. * confidence: better shape defs * use ModelConstants class * fix confidence * Parser * slightly more power too * no inline ifs :( * confidence: just use if statements old-commit-hash: cad17b125595c4654bfd8299b041b94ccb3faf73
This commit is contained in:
YassineYousfi
@@ -79,6 +79,7 @@ comma*.sh
|
||||
|
||||
selfdrive/modeld/thneed/compile
|
||||
selfdrive/modeld/models/*.thneed
|
||||
selfdrive/modeld/models/*.pkl
|
||||
|
||||
*.bz2
|
||||
|
||||
|
||||
@@ -358,6 +358,9 @@ selfdrive/modeld/.gitignore
|
||||
selfdrive/modeld/__init__.py
|
||||
selfdrive/modeld/SConscript
|
||||
selfdrive/modeld/modeld.py
|
||||
selfdrive/modeld/parse_model_outputs.py
|
||||
selfdrive/modeld/fill_model_msg.py
|
||||
selfdrive/modeld/get_model_metadata.py
|
||||
selfdrive/modeld/navmodeld.py
|
||||
selfdrive/modeld/dmonitoringmodeld.py
|
||||
selfdrive/modeld/constants.py
|
||||
@@ -370,8 +373,6 @@ selfdrive/modeld/models/*.pyx
|
||||
selfdrive/modeld/models/commonmodel.cc
|
||||
selfdrive/modeld/models/commonmodel.h
|
||||
|
||||
selfdrive/modeld/models/driving.cc
|
||||
selfdrive/modeld/models/driving.h
|
||||
selfdrive/modeld/models/supercombo.onnx
|
||||
|
||||
selfdrive/modeld/models/dmonitoring_model_q.dlc
|
||||
|
||||
@@ -4,7 +4,7 @@ from cereal import car, log
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
# WARNING: this value was determined based on the model's training distribution,
|
||||
# model predictions above this speed can be unpredictable
|
||||
@@ -177,7 +177,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
|
||||
# in high delay cases some corrections never even get commanded. So just use
|
||||
# psi to calculate a simple linearization of desired curvature
|
||||
current_curvature_desired = curvatures[0]
|
||||
psi = interp(delay, T_IDXS[:CONTROL_N], psis)
|
||||
psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
|
||||
average_curvature_desired = psi / (v_ego * delay)
|
||||
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
|
||||
|
||||
|
||||
@@ -5,7 +5,7 @@ import numpy as np
|
||||
|
||||
from casadi import SX, vertcat, sin, cos
|
||||
# WARNING: imports outside of constants will not trigger a rebuild
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
if __name__ == '__main__': # generating code
|
||||
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
@@ -66,7 +66,7 @@ def gen_lat_ocp():
|
||||
ocp = AcadosOcp()
|
||||
ocp.model = gen_lat_model()
|
||||
|
||||
Tf = np.array(T_IDXS)[N]
|
||||
Tf = np.array(ModelConstants.T_IDXS)[N]
|
||||
|
||||
# set dimensions
|
||||
ocp.dims.N = N
|
||||
@@ -122,7 +122,7 @@ def gen_lat_ocp():
|
||||
|
||||
# set prediction horizon
|
||||
ocp.solver_options.tf = Tf
|
||||
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
|
||||
ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
|
||||
|
||||
ocp.code_export_directory = EXPORT_DIR
|
||||
return ocp
|
||||
|
||||
@@ -3,7 +3,7 @@ from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
|
||||
from openpilot.selfdrive.controls.lib.pid import PIDController
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
@@ -70,19 +70,19 @@ class LongControl:
|
||||
# Interp control trajectory
|
||||
speeds = long_plan.speeds
|
||||
if len(speeds) == CONTROL_N:
|
||||
v_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_now = interp(t_since_plan, T_IDXS[:CONTROL_N], long_plan.accels)
|
||||
v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
|
||||
|
||||
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now
|
||||
|
||||
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, T_IDXS[:CONTROL_N], speeds)
|
||||
v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds)
|
||||
a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now
|
||||
|
||||
v_target = min(v_target_lower, v_target_upper)
|
||||
a_target = min(a_target_lower, a_target_upper)
|
||||
|
||||
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, T_IDXS[:CONTROL_N], speeds)
|
||||
v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds)
|
||||
else:
|
||||
v_target = 0.0
|
||||
v_target_now = 0.0
|
||||
|
||||
@@ -9,7 +9,7 @@ import cereal.messaging as messaging
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
|
||||
@@ -76,9 +76,9 @@ class LongitudinalPlanner:
|
||||
if (len(model_msg.position.x) == 33 and
|
||||
len(model_msg.velocity.x) == 33 and
|
||||
len(model_msg.acceleration.x) == 33):
|
||||
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
|
||||
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
|
||||
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
|
||||
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
|
||||
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
|
||||
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
|
||||
j = np.zeros(len(T_IDXS_MPC))
|
||||
else:
|
||||
x = np.zeros(len(T_IDXS_MPC))
|
||||
@@ -135,11 +135,11 @@ class LongitudinalPlanner:
|
||||
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
|
||||
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
|
||||
|
||||
self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
|
||||
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
|
||||
self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
|
||||
self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
|
||||
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
|
||||
|
||||
# TODO counter is only needed because radar is glitchy, remove once radar is gone
|
||||
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
|
||||
@@ -148,7 +148,7 @@ class LongitudinalPlanner:
|
||||
|
||||
# Interpolate 0.05 seconds and save as starting point for next iteration
|
||||
a_prev = self.a_desired
|
||||
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
|
||||
self.a_desired = float(interp(DT_MDL, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
def publish(self, sm, pm):
|
||||
|
||||
@@ -5,7 +5,7 @@ from cereal import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import Priority, config_realtime_process
|
||||
from openpilot.system.swaglog import cloudlog
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
|
||||
import cereal.messaging as messaging
|
||||
@@ -14,8 +14,8 @@ def cumtrapz(x, t):
|
||||
return np.concatenate([[0], np.cumsum(((x[0:-1] + x[1:])/2) * np.diff(t))])
|
||||
|
||||
def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
|
||||
plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, T_IDXS)
|
||||
model_odo = cumtrapz(lateral_planner.v_plan, T_IDXS)
|
||||
plan_odo = cumtrapz(longitudinal_planner.v_desired_trajectory_full, ModelConstants.T_IDXS)
|
||||
model_odo = cumtrapz(lateral_planner.v_plan, ModelConstants.T_IDXS)
|
||||
|
||||
ui_send = messaging.new_message('uiPlan')
|
||||
ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
|
||||
|
||||
@@ -45,17 +45,19 @@ snpe_rpath = lenvCython['RPATH'] + [snpe_rpath_qcom if arch == "larch64" else sn
|
||||
cython_libs = envCython["LIBS"] + libs
|
||||
snpemodel_lib = lenv.Library('snpemodel', ['runners/snpemodel.cc'])
|
||||
commonmodel_lib = lenv.Library('commonmodel', common_src)
|
||||
driving_lib = lenv.Library('driving', ['models/driving.cc'])
|
||||
|
||||
lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=cython_libs, FRAMEWORKS=frameworks)
|
||||
lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath)
|
||||
lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
|
||||
lenvCython.Program('models/driving_pyx.so', 'models/driving_pyx.pyx', LIBS=[driving_lib, commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
|
||||
|
||||
# Get model metadata
|
||||
fn = File("models/supercombo").abspath
|
||||
cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx'
|
||||
files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.endswith("get_model_metadata.py")], [])
|
||||
lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"]+files, cmd)
|
||||
|
||||
# Build thneed model
|
||||
if arch == "larch64" or GetOption('pc_thneed'):
|
||||
fn = File("models/supercombo").abspath
|
||||
|
||||
tinygrad_opts = ["NOLOCALS=1", "IMAGE=2", "GPU=1"]
|
||||
if not GetOption('pc_thneed'):
|
||||
# use FLOAT16 on device for speed + don't cache the CL kernels for space
|
||||
|
||||
@@ -1,7 +1,78 @@
|
||||
IDX_N = 33
|
||||
import numpy as np
|
||||
|
||||
def index_function(idx, max_val=192, max_idx=32):
|
||||
return (max_val) * ((idx/max_idx)**2)
|
||||
|
||||
class ModelConstants:
|
||||
# time and distance indices
|
||||
IDX_N = 33
|
||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
|
||||
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
|
||||
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
|
||||
LEAD_T_OFFSETS = [0., 2., 4.]
|
||||
META_T_IDXS = [2., 4., 6., 8., 10.]
|
||||
|
||||
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
|
||||
# model inputs constants
|
||||
MODEL_FREQ = 20
|
||||
FEATURE_LEN = 512
|
||||
HISTORY_BUFFER_LEN = 99
|
||||
DESIRE_LEN = 8
|
||||
TRAFFIC_CONVENTION_LEN = 2
|
||||
NAV_FEATURE_LEN = 256
|
||||
NAV_INSTRUCTION_LEN = 150
|
||||
DRIVING_STYLE_LEN = 12
|
||||
|
||||
# model outputs constants
|
||||
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
|
||||
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
|
||||
|
||||
DISENGAGE_WIDTH = 5
|
||||
POSE_WIDTH = 6
|
||||
WIDE_FROM_DEVICE_WIDTH = 3
|
||||
SIM_POSE_WIDTH = 6
|
||||
LEAD_WIDTH = 4
|
||||
LANE_LINES_WIDTH = 2
|
||||
ROAD_EDGES_WIDTH = 2
|
||||
PLAN_WIDTH = 15
|
||||
DESIRE_PRED_WIDTH = 8
|
||||
|
||||
NUM_LANE_LINES = 4
|
||||
NUM_ROAD_EDGES = 2
|
||||
|
||||
LEAD_TRAJ_LEN = 6
|
||||
DESIRE_PRED_LEN = 4
|
||||
|
||||
PLAN_MHP_N = 5
|
||||
LEAD_MHP_N = 2
|
||||
PLAN_MHP_SELECTION = 1
|
||||
LEAD_MHP_SELECTION = 3
|
||||
|
||||
FCW_THRESHOLD_5MS2_HIGH = 0.15
|
||||
FCW_THRESHOLD_5MS2_LOW = 0.05
|
||||
FCW_THRESHOLD_3MS2 = 0.7
|
||||
|
||||
CONFIDENCE_BUFFER_LEN = 5
|
||||
RYG_GREEN = 0.01165
|
||||
RYG_YELLOW = 0.06157
|
||||
|
||||
# model outputs slices
|
||||
class Plan:
|
||||
POSITION = slice(0, 3)
|
||||
VELOCITY = slice(3, 6)
|
||||
ACCELERATION = slice(6, 9)
|
||||
T_FROM_CURRENT_EULER = slice(9, 12)
|
||||
ORIENTATION_RATE = slice(12, 15)
|
||||
|
||||
class Meta:
|
||||
ENGAGED = slice(0, 1)
|
||||
# next 2, 4, 6, 8, 10 seconds
|
||||
GAS_DISENGAGE = slice(1, 36, 7)
|
||||
BRAKE_DISENGAGE = slice(2, 36, 7)
|
||||
STEER_OVERRIDE = slice(3, 36, 7)
|
||||
HARD_BRAKE_3 = slice(4, 36, 7)
|
||||
HARD_BRAKE_4 = slice(5, 36, 7)
|
||||
HARD_BRAKE_5 = slice(6, 36, 7)
|
||||
GAS_PRESS = slice(7, 36, 7)
|
||||
# next 0, 2, 4, 6, 8, 10 seconds
|
||||
LEFT_BLINKER = slice(36, 48, 2)
|
||||
RIGHT_BLINKER = slice(37, 48, 2)
|
||||
|
||||
@@ -0,0 +1,181 @@
|
||||
import capnp
|
||||
import numpy as np
|
||||
from typing import Dict
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
|
||||
|
||||
ConfidenceClass = log.ModelDataV2.ConfidenceClass
|
||||
|
||||
class PublishState:
|
||||
def __init__(self):
|
||||
self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_5ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
|
||||
self.prev_brake_3ms2_probs = np.zeros(ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
|
||||
|
||||
def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.z = z.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if z_std is not None:
|
||||
builder.zStd = z_std.tolist()
|
||||
|
||||
def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
|
||||
builder.t = t
|
||||
builder.x = x.tolist()
|
||||
builder.y = y.tolist()
|
||||
builder.v = v.tolist()
|
||||
builder.a = a.tolist()
|
||||
if x_std is not None:
|
||||
builder.xStd = x_std.tolist()
|
||||
if y_std is not None:
|
||||
builder.yStd = y_std.tolist()
|
||||
if v_std is not None:
|
||||
builder.vStd = v_std.tolist()
|
||||
if a_std is not None:
|
||||
builder.aStd = a_std.tolist()
|
||||
|
||||
def fill_model_msg(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, timestamp_llk: int, model_execution_time: float,
|
||||
nav_enabled: bool, valid: bool) -> None:
|
||||
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
|
||||
msg.valid = valid
|
||||
|
||||
modelV2 = msg.modelV2
|
||||
modelV2.frameId = vipc_frame_id
|
||||
modelV2.frameIdExtra = vipc_frame_id_extra
|
||||
modelV2.frameAge = frame_age
|
||||
modelV2.frameDropPerc = frame_drop * 100
|
||||
modelV2.timestampEof = timestamp_eof
|
||||
modelV2.locationMonoTime = timestamp_llk
|
||||
modelV2.modelExecutionTime = model_execution_time
|
||||
modelV2.navEnabled = nav_enabled
|
||||
|
||||
# plan
|
||||
position = modelV2.position
|
||||
fill_xyzt(position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
|
||||
velocity = modelV2.velocity
|
||||
fill_xyzt(velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
|
||||
acceleration = modelV2.acceleration
|
||||
fill_xyzt(acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
|
||||
orientation = modelV2.orientation
|
||||
fill_xyzt(orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
|
||||
orientation_rate = modelV2.orientationRate
|
||||
fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
|
||||
|
||||
# times at X_IDXS according to model plan
|
||||
PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
|
||||
PLAN_T_IDXS[0] = 0.0
|
||||
plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist()
|
||||
for xidx in range(1, ModelConstants.IDX_N):
|
||||
tidx = 0
|
||||
# increment tidx until we find an element that's further away than the current xidx
|
||||
while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]:
|
||||
tidx += 1
|
||||
if tidx == ModelConstants.IDX_N - 1:
|
||||
# if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
|
||||
PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1]
|
||||
break
|
||||
# interpolate to find `t` for the current xidx
|
||||
current_x_val = plan_x[tidx]
|
||||
next_x_val = plan_x[tidx+1]
|
||||
p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val)
|
||||
PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
|
||||
|
||||
# lane lines
|
||||
modelV2.init('laneLines', 4)
|
||||
for i in range(4):
|
||||
lane_line = modelV2.laneLines[i]
|
||||
fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
|
||||
modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
|
||||
modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
|
||||
|
||||
# road edges
|
||||
modelV2.init('roadEdges', 2)
|
||||
for i in range(2):
|
||||
road_edge = modelV2.roadEdges[i]
|
||||
fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
|
||||
modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
|
||||
|
||||
# leads
|
||||
modelV2.init('leadsV3', 3)
|
||||
for i in range(3):
|
||||
lead = modelV2.leadsV3[i]
|
||||
fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
|
||||
lead.prob = net_output_data['lead_prob'][0,i].tolist()
|
||||
lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
|
||||
|
||||
# meta
|
||||
meta = modelV2.meta
|
||||
meta.desireState = net_output_data['desire_state'][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.init('disengagePredictions')
|
||||
disengage_predictions = meta.disengagePredictions
|
||||
disengage_predictions.t = ModelConstants.META_T_IDXS
|
||||
disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
|
||||
disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
|
||||
disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
|
||||
disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
|
||||
disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
|
||||
disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
|
||||
|
||||
publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
|
||||
publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
|
||||
publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
|
||||
publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
|
||||
hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
|
||||
(publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
|
||||
meta.hardBrakePredicted = hard_brake_predicted.item()
|
||||
|
||||
# temporal pose
|
||||
temporal_pose = modelV2.temporalPose
|
||||
temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
|
||||
temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
|
||||
temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
|
||||
temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
|
||||
|
||||
# confidence
|
||||
if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
|
||||
# any disengage prob
|
||||
brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
|
||||
gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
|
||||
steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
|
||||
any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
|
||||
# independent disengage prob for each 2s slice
|
||||
ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
|
||||
# rolling buf for 2, 4, 6, 8, 10s
|
||||
publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
|
||||
publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
|
||||
|
||||
score = 0.
|
||||
for i in range(ModelConstants.DISENGAGE_WIDTH):
|
||||
score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
|
||||
if score < ModelConstants.RYG_GREEN:
|
||||
modelV2.confidence = ConfidenceClass.green
|
||||
elif score < ModelConstants.RYG_YELLOW:
|
||||
modelV2.confidence = ConfidenceClass.yellow
|
||||
else:
|
||||
modelV2.confidence = ConfidenceClass.red
|
||||
|
||||
def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray],
|
||||
vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None:
|
||||
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
|
||||
cameraOdometry = msg.cameraOdometry
|
||||
|
||||
cameraOdometry.frameId = vipc_frame_id
|
||||
cameraOdometry.timestampEof = timestamp_eof
|
||||
|
||||
cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
|
||||
cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
|
||||
cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
|
||||
cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
|
||||
cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
|
||||
cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()
|
||||
Executable
+29
@@ -0,0 +1,29 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import pathlib
|
||||
import onnx
|
||||
import codecs
|
||||
import pickle
|
||||
from typing import Tuple
|
||||
|
||||
def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]:
|
||||
shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
|
||||
name = value_info.name
|
||||
return name, shape
|
||||
|
||||
if __name__ == "__main__":
|
||||
model_path = pathlib.Path(sys.argv[1])
|
||||
model = onnx.load(str(model_path))
|
||||
i = [x.key for x in model.metadata_props].index('output_slices')
|
||||
output_slices = model.metadata_props[i].value
|
||||
|
||||
metadata = {}
|
||||
metadata['output_slices'] = pickle.loads(codecs.decode(output_slices.encode(), "base64"))
|
||||
metadata['input_shapes'] = dict([get_name_and_shape(x) for x in model.graph.input])
|
||||
metadata['output_shapes'] = dict([get_name_and_shape(x) for x in model.graph.output])
|
||||
|
||||
metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
|
||||
with open(metadata_path, 'wb') as f:
|
||||
pickle.dump(metadata, f)
|
||||
|
||||
print(f'saved metadata to {metadata_path}')
|
||||
+46
-30
@@ -1,7 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import time
|
||||
import pickle
|
||||
import numpy as np
|
||||
import cereal.messaging as messaging
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional
|
||||
from setproctitle import setproctitle
|
||||
@@ -13,16 +15,17 @@ from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.realtime import config_realtime_process
|
||||
from openpilot.common.transformations.model import get_warp_matrix
|
||||
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
|
||||
from openpilot.selfdrive.modeld.parse_model_outputs import Parser
|
||||
from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
|
||||
from openpilot.selfdrive.modeld.models.driving_pyx import (
|
||||
PublishState, create_model_msg, create_pose_msg,
|
||||
FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN,
|
||||
OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
|
||||
|
||||
MODEL_PATHS = {
|
||||
ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
|
||||
|
||||
METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
|
||||
|
||||
class FrameMeta:
|
||||
frame_id: int = 0
|
||||
timestamp_sof: int = 0
|
||||
@@ -43,28 +46,38 @@ class ModelState:
|
||||
def __init__(self, context: CLContext):
|
||||
self.frame = ModelFrame(context)
|
||||
self.wide_frame = ModelFrame(context)
|
||||
self.prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
|
||||
self.output = np.zeros(NET_OUTPUT_SIZE, dtype=np.float32)
|
||||
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
self.inputs = {
|
||||
'desire': np.zeros(DESIRE_LEN * (HISTORY_BUFFER_LEN+1), dtype=np.float32),
|
||||
'traffic_convention': np.zeros(TRAFFIC_CONVENTION_LEN, dtype=np.float32),
|
||||
'nav_features': np.zeros(NAV_FEATURE_LEN, dtype=np.float32),
|
||||
'nav_instructions': np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32),
|
||||
'features_buffer': np.zeros(HISTORY_BUFFER_LEN * FEATURE_LEN, dtype=np.float32),
|
||||
'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),
|
||||
'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
|
||||
'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
|
||||
'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
|
||||
}
|
||||
|
||||
with open(METADATA_PATH, 'rb') as f:
|
||||
model_metadata = pickle.load(f)
|
||||
|
||||
self.output_slices = model_metadata['output_slices']
|
||||
net_output_size = model_metadata['output_shapes']['outputs'][1]
|
||||
self.output = np.zeros(net_output_size, dtype=np.float32)
|
||||
self.parser = Parser()
|
||||
|
||||
self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.GPU, False, context)
|
||||
self.model.addInput("input_imgs", None)
|
||||
self.model.addInput("big_input_imgs", None)
|
||||
for k,v in self.inputs.items():
|
||||
self.model.addInput(k, v)
|
||||
|
||||
def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]:
|
||||
return {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()}
|
||||
|
||||
def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray,
|
||||
inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[np.ndarray]:
|
||||
inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]:
|
||||
# Model decides when action is completed, so desire input is just a pulse triggered on rising edge
|
||||
inputs['desire'][0] = 0
|
||||
self.inputs['desire'][:-DESIRE_LEN] = self.inputs['desire'][DESIRE_LEN:]
|
||||
self.inputs['desire'][-DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
|
||||
self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
|
||||
self.prev_desire[:] = inputs['desire']
|
||||
|
||||
self.inputs['traffic_convention'][:] = inputs['traffic_convention']
|
||||
@@ -81,9 +94,11 @@ class ModelState:
|
||||
return None
|
||||
|
||||
self.model.execute()
|
||||
self.inputs['features_buffer'][:-FEATURE_LEN] = self.inputs['features_buffer'][FEATURE_LEN:]
|
||||
self.inputs['features_buffer'][-FEATURE_LEN:] = self.output[OUTPUT_SIZE:OUTPUT_SIZE+FEATURE_LEN]
|
||||
return self.output
|
||||
outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
|
||||
|
||||
self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
|
||||
self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
|
||||
return outputs
|
||||
|
||||
|
||||
def main():
|
||||
@@ -122,22 +137,21 @@ def main():
|
||||
pm = PubMaster(["modelV2", "cameraOdometry"])
|
||||
sm = SubMaster(["lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction"])
|
||||
|
||||
state = PublishState()
|
||||
publish_state = PublishState()
|
||||
params = Params()
|
||||
|
||||
# setup filter to track dropped frames
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / MODEL_FREQ)
|
||||
frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
|
||||
frame_id = 0
|
||||
last_vipc_frame_id = 0
|
||||
run_count = 0
|
||||
# last = 0.0
|
||||
|
||||
model_transform_main = np.zeros((3, 3), dtype=np.float32)
|
||||
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
|
||||
live_calib_seen = False
|
||||
driving_style = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], dtype=np.float32)
|
||||
nav_features = np.zeros(NAV_FEATURE_LEN, dtype=np.float32)
|
||||
nav_instructions = np.zeros(NAV_INSTRUCTION_LEN, dtype=np.float32)
|
||||
nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
|
||||
nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
|
||||
buf_main, buf_extra = None, None
|
||||
meta_main = FrameMeta()
|
||||
meta_extra = FrameMeta()
|
||||
@@ -190,8 +204,8 @@ def main():
|
||||
traffic_convention = np.zeros(2)
|
||||
traffic_convention[int(is_rhd)] = 1
|
||||
|
||||
vec_desire = np.zeros(DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < DESIRE_LEN:
|
||||
vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
|
||||
if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
|
||||
vec_desire[desire] = 1
|
||||
|
||||
# Enable/disable nav features
|
||||
@@ -244,13 +258,15 @@ def main():
|
||||
model_execution_time = mt2 - mt1
|
||||
|
||||
if model_output is not None:
|
||||
pm.send("modelV2", create_model_msg(model_output, state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
|
||||
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen))
|
||||
pm.send("cameraOdometry", create_pose_msg(model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen))
|
||||
modelv2_send = messaging.new_message('modelV2')
|
||||
posenet_send = messaging.new_message('cameraOdometry')
|
||||
fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
|
||||
meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
|
||||
|
||||
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('cameraOdometry', posenet_send)
|
||||
|
||||
# print("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f" %
|
||||
# ((mt2 - mt1)*1000, (mt1 - last)*1000, meta_extra.frame_id, frame_id, frame_drop_ratio))
|
||||
# last = mt1
|
||||
last_vipc_frame_id = meta_main.frame_id
|
||||
|
||||
|
||||
|
||||
@@ -20,7 +20,11 @@ To view the architecture of the ONNX networks, you can use [netron](https://netr
|
||||
* **traffic convention**
|
||||
* one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
|
||||
* **feature buffer**
|
||||
* A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 128
|
||||
* A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
|
||||
* **nav features**
|
||||
* 1 * 150
|
||||
* **nav instructions**
|
||||
* 1 * 256
|
||||
|
||||
|
||||
### Supercombo output format (Full size: XXX x float32)
|
||||
|
||||
@@ -1,330 +0,0 @@
|
||||
#include "selfdrive/modeld/models/driving.h"
|
||||
|
||||
#include <cstring>
|
||||
|
||||
|
||||
void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) {
|
||||
std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
|
||||
const auto &best_prediction = leads.get_best_prediction(t_idx);
|
||||
lead.setProb(sigmoid(leads.prob[t_idx]));
|
||||
lead.setProbTime(prob_t);
|
||||
std::array<float, LEAD_TRAJ_LEN> lead_x, lead_y, lead_v, lead_a;
|
||||
std::array<float, LEAD_TRAJ_LEN> lead_x_std, lead_y_std, lead_v_std, lead_a_std;
|
||||
for (int i=0; i<LEAD_TRAJ_LEN; i++) {
|
||||
lead_x[i] = best_prediction.mean[i].x;
|
||||
lead_y[i] = best_prediction.mean[i].y;
|
||||
lead_v[i] = best_prediction.mean[i].velocity;
|
||||
lead_a[i] = best_prediction.mean[i].acceleration;
|
||||
lead_x_std[i] = exp(best_prediction.std[i].x);
|
||||
lead_y_std[i] = exp(best_prediction.std[i].y);
|
||||
lead_v_std[i] = exp(best_prediction.std[i].velocity);
|
||||
lead_a_std[i] = exp(best_prediction.std[i].acceleration);
|
||||
}
|
||||
lead.setT(to_kj_array_ptr(lead_t));
|
||||
lead.setX(to_kj_array_ptr(lead_x));
|
||||
lead.setY(to_kj_array_ptr(lead_y));
|
||||
lead.setV(to_kj_array_ptr(lead_v));
|
||||
lead.setA(to_kj_array_ptr(lead_a));
|
||||
lead.setXStd(to_kj_array_ptr(lead_x_std));
|
||||
lead.setYStd(to_kj_array_ptr(lead_y_std));
|
||||
lead.setVStd(to_kj_array_ptr(lead_v_std));
|
||||
lead.setAStd(to_kj_array_ptr(lead_a_std));
|
||||
}
|
||||
|
||||
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data, PublishState &ps) {
|
||||
std::array<float, DESIRE_LEN> desire_state_softmax;
|
||||
softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
|
||||
|
||||
std::array<float, DESIRE_PRED_LEN * DESIRE_LEN> desire_pred_softmax;
|
||||
for (int i=0; i<DESIRE_PRED_LEN; i++) {
|
||||
softmax(meta_data.desire_pred_prob[i].array.data(), desire_pred_softmax.data() + (i * DESIRE_LEN), DESIRE_LEN);
|
||||
}
|
||||
|
||||
std::array<float, DISENGAGE_LEN> lat_long_t = {2, 4, 6, 8, 10};
|
||||
std::array<float, DISENGAGE_LEN> gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid,
|
||||
brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid;
|
||||
for (int i=0; i<DISENGAGE_LEN; i++) {
|
||||
gas_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_disengage);
|
||||
brake_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_disengage);
|
||||
steer_override_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].steer_override);
|
||||
brake_3ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_3ms2);
|
||||
brake_4ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_4ms2);
|
||||
brake_5ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_5ms2);
|
||||
//gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
|
||||
}
|
||||
|
||||
std::memmove(ps.prev_brake_5ms2_probs.data(), &ps.prev_brake_5ms2_probs[1], 4*sizeof(float));
|
||||
std::memmove(ps.prev_brake_3ms2_probs.data(), &ps.prev_brake_3ms2_probs[1], 2*sizeof(float));
|
||||
ps.prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
|
||||
ps.prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
|
||||
|
||||
bool above_fcw_threshold = true;
|
||||
for (int i=0; i<ps.prev_brake_5ms2_probs.size(); i++) {
|
||||
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
|
||||
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_5ms2_probs[i] > threshold;
|
||||
}
|
||||
for (int i=0; i<ps.prev_brake_3ms2_probs.size(); i++) {
|
||||
above_fcw_threshold = above_fcw_threshold && ps.prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
|
||||
}
|
||||
|
||||
auto disengage = meta.initDisengagePredictions();
|
||||
disengage.setT(to_kj_array_ptr(lat_long_t));
|
||||
disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid));
|
||||
disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid));
|
||||
disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid));
|
||||
disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid));
|
||||
disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid));
|
||||
disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid));
|
||||
|
||||
meta.setEngagedProb(sigmoid(meta_data.engaged_prob));
|
||||
meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax));
|
||||
meta.setDesireState(to_kj_array_ptr(desire_state_softmax));
|
||||
meta.setHardBrakePredicted(above_fcw_threshold);
|
||||
}
|
||||
|
||||
void fill_confidence(cereal::ModelDataV2::Builder &framed, PublishState &ps) {
|
||||
if (framed.getFrameId() % (2*MODEL_FREQ) == 0) {
|
||||
// update every 2s to match predictions interval
|
||||
auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs();
|
||||
auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs();
|
||||
auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs();
|
||||
|
||||
float any_dp[DISENGAGE_LEN];
|
||||
float dp_ind[DISENGAGE_LEN];
|
||||
|
||||
for (int i = 0; i < DISENGAGE_LEN; i++) {
|
||||
any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob
|
||||
}
|
||||
|
||||
dp_ind[0] = any_dp[0];
|
||||
for (int i = 0; i < DISENGAGE_LEN-1; i++) {
|
||||
dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice
|
||||
}
|
||||
|
||||
// rolling buf for 2, 4, 6, 8, 10s
|
||||
std::memmove(&ps.disengage_buffer[0], &ps.disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1));
|
||||
std::memcpy(&ps.disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN);
|
||||
}
|
||||
|
||||
float score = 0;
|
||||
for (int i = 0; i < DISENGAGE_LEN; i++) {
|
||||
score += ps.disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN;
|
||||
}
|
||||
|
||||
if (score < RYG_GREEN) {
|
||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN);
|
||||
} else if (score < RYG_YELLOW) {
|
||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW);
|
||||
} else {
|
||||
framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED);
|
||||
}
|
||||
}
|
||||
|
||||
template<size_t size>
|
||||
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
|
||||
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
|
||||
xyzt.setT(to_kj_array_ptr(t));
|
||||
xyzt.setX(to_kj_array_ptr(x));
|
||||
xyzt.setY(to_kj_array_ptr(y));
|
||||
xyzt.setZ(to_kj_array_ptr(z));
|
||||
}
|
||||
|
||||
template<size_t size>
|
||||
void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
|
||||
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
|
||||
const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
|
||||
fill_xyzt(xyzt, t, x, y, z);
|
||||
xyzt.setXStd(to_kj_array_ptr(x_std));
|
||||
xyzt.setYStd(to_kj_array_ptr(y_std));
|
||||
xyzt.setZStd(to_kj_array_ptr(z_std));
|
||||
}
|
||||
|
||||
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) {
|
||||
std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
|
||||
std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
|
||||
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
|
||||
std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
|
||||
std::array<float, TRAJECTORY_SIZE> acc_x, acc_y, acc_z;
|
||||
std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
|
||||
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
pos_x[i] = plan.mean[i].position.x;
|
||||
pos_y[i] = plan.mean[i].position.y;
|
||||
pos_z[i] = plan.mean[i].position.z;
|
||||
pos_x_std[i] = exp(plan.std[i].position.x);
|
||||
pos_y_std[i] = exp(plan.std[i].position.y);
|
||||
pos_z_std[i] = exp(plan.std[i].position.z);
|
||||
vel_x[i] = plan.mean[i].velocity.x;
|
||||
vel_y[i] = plan.mean[i].velocity.y;
|
||||
vel_z[i] = plan.mean[i].velocity.z;
|
||||
acc_x[i] = plan.mean[i].acceleration.x;
|
||||
acc_y[i] = plan.mean[i].acceleration.y;
|
||||
acc_z[i] = plan.mean[i].acceleration.z;
|
||||
rot_x[i] = plan.mean[i].rotation.x;
|
||||
rot_y[i] = plan.mean[i].rotation.y;
|
||||
rot_z[i] = plan.mean[i].rotation.z;
|
||||
rot_rate_x[i] = plan.mean[i].rotation_rate.x;
|
||||
rot_rate_y[i] = plan.mean[i].rotation_rate.y;
|
||||
rot_rate_z[i] = plan.mean[i].rotation_rate.z;
|
||||
}
|
||||
|
||||
fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
|
||||
fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
|
||||
fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, acc_x, acc_y, acc_z);
|
||||
fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
|
||||
fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
|
||||
}
|
||||
|
||||
void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
|
||||
const ModelOutputLaneLines &lanes) {
|
||||
std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
|
||||
std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
|
||||
std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
|
||||
std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
|
||||
for (int j=0; j<TRAJECTORY_SIZE; j++) {
|
||||
left_far_y[j] = lanes.mean.left_far[j].y;
|
||||
left_far_z[j] = lanes.mean.left_far[j].z;
|
||||
left_near_y[j] = lanes.mean.left_near[j].y;
|
||||
left_near_z[j] = lanes.mean.left_near[j].z;
|
||||
right_near_y[j] = lanes.mean.right_near[j].y;
|
||||
right_near_z[j] = lanes.mean.right_near[j].z;
|
||||
right_far_y[j] = lanes.mean.right_far[j].y;
|
||||
right_far_z[j] = lanes.mean.right_far[j].z;
|
||||
}
|
||||
|
||||
auto lane_lines = framed.initLaneLines(4);
|
||||
fill_xyzt(lane_lines[0], plan_t, X_IDXS_FLOAT, left_far_y, left_far_z);
|
||||
fill_xyzt(lane_lines[1], plan_t, X_IDXS_FLOAT, left_near_y, left_near_z);
|
||||
fill_xyzt(lane_lines[2], plan_t, X_IDXS_FLOAT, right_near_y, right_near_z);
|
||||
fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z);
|
||||
|
||||
framed.setLaneLineStds({
|
||||
exp(lanes.std.left_far[0].y),
|
||||
exp(lanes.std.left_near[0].y),
|
||||
exp(lanes.std.right_near[0].y),
|
||||
exp(lanes.std.right_far[0].y),
|
||||
});
|
||||
|
||||
framed.setLaneLineProbs({
|
||||
sigmoid(lanes.prob.left_far.val),
|
||||
sigmoid(lanes.prob.left_near.val),
|
||||
sigmoid(lanes.prob.right_near.val),
|
||||
sigmoid(lanes.prob.right_far.val),
|
||||
});
|
||||
}
|
||||
|
||||
void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
|
||||
const ModelOutputRoadEdges &edges) {
|
||||
std::array<float, TRAJECTORY_SIZE> left_y, left_z;
|
||||
std::array<float, TRAJECTORY_SIZE> right_y, right_z;
|
||||
for (int j=0; j<TRAJECTORY_SIZE; j++) {
|
||||
left_y[j] = edges.mean.left[j].y;
|
||||
left_z[j] = edges.mean.left[j].z;
|
||||
right_y[j] = edges.mean.right[j].y;
|
||||
right_z[j] = edges.mean.right[j].z;
|
||||
}
|
||||
|
||||
auto road_edges = framed.initRoadEdges(2);
|
||||
fill_xyzt(road_edges[0], plan_t, X_IDXS_FLOAT, left_y, left_z);
|
||||
fill_xyzt(road_edges[1], plan_t, X_IDXS_FLOAT, right_y, right_z);
|
||||
|
||||
framed.setRoadEdgeStds({
|
||||
exp(edges.std.left[0].y),
|
||||
exp(edges.std.right[0].y),
|
||||
});
|
||||
}
|
||||
|
||||
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs, PublishState &ps) {
|
||||
const auto &best_plan = net_outputs.plans.get_best_prediction();
|
||||
std::array<float, TRAJECTORY_SIZE> plan_t;
|
||||
std::fill_n(plan_t.data(), plan_t.size(), NAN);
|
||||
plan_t[0] = 0.0;
|
||||
for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
|
||||
// increment tidx until we find an element that's further away than the current xidx
|
||||
for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
|
||||
tidx++;
|
||||
}
|
||||
if (tidx == TRAJECTORY_SIZE - 1) {
|
||||
// if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
|
||||
plan_t[xidx] = T_IDXS[TRAJECTORY_SIZE - 1];
|
||||
break;
|
||||
}
|
||||
|
||||
// interpolate to find `t` for the current xidx
|
||||
float current_x_val = best_plan.mean[tidx].position.x;
|
||||
float next_x_val = best_plan.mean[tidx+1].position.x;
|
||||
float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
|
||||
plan_t[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
|
||||
}
|
||||
|
||||
fill_plan(framed, best_plan);
|
||||
fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
|
||||
fill_road_edges(framed, plan_t, net_outputs.road_edges);
|
||||
|
||||
// meta
|
||||
fill_meta(framed.initMeta(), net_outputs.meta, ps);
|
||||
|
||||
// confidence
|
||||
fill_confidence(framed, ps);
|
||||
|
||||
// leads
|
||||
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
|
||||
std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
|
||||
for (int i=0; i<LEAD_MHP_SELECTION; i++) {
|
||||
fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]);
|
||||
}
|
||||
|
||||
// temporal pose
|
||||
const auto &v_mean = net_outputs.temporal_pose.velocity_mean;
|
||||
const auto &r_mean = net_outputs.temporal_pose.rotation_mean;
|
||||
const auto &v_std = net_outputs.temporal_pose.velocity_std;
|
||||
const auto &r_std = net_outputs.temporal_pose.rotation_std;
|
||||
auto temporal_pose = framed.initTemporalPose();
|
||||
temporal_pose.setTrans({v_mean.x, v_mean.y, v_mean.z});
|
||||
temporal_pose.setRot({r_mean.x, r_mean.y, r_mean.z});
|
||||
temporal_pose.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
|
||||
temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
|
||||
}
|
||||
|
||||
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
|
||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid) {
|
||||
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||
auto framed = msg.initEvent(valid).initModelV2();
|
||||
framed.setFrameId(vipc_frame_id);
|
||||
framed.setFrameIdExtra(vipc_frame_id_extra);
|
||||
framed.setFrameAge(frame_age);
|
||||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
framed.setLocationMonoTime(timestamp_llk);
|
||||
framed.setModelExecutionTime(model_execution_time);
|
||||
framed.setNavEnabled(nav_enabled);
|
||||
if (send_raw_pred) {
|
||||
framed.setRawPredictions(kj::ArrayPtr<const float>(net_output_data, NET_OUTPUT_SIZE).asBytes());
|
||||
}
|
||||
fill_model(framed, *((ModelOutput*) net_output_data), ps);
|
||||
}
|
||||
|
||||
void fill_pose_msg(MessageBuilder &msg, float *net_output_data, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid) {
|
||||
const ModelOutput &net_outputs = *((ModelOutput*) net_output_data);
|
||||
const auto &v_mean = net_outputs.pose.velocity_mean;
|
||||
const auto &r_mean = net_outputs.pose.rotation_mean;
|
||||
const auto &t_mean = net_outputs.wide_from_device_euler.mean;
|
||||
const auto &v_std = net_outputs.pose.velocity_std;
|
||||
const auto &r_std = net_outputs.pose.rotation_std;
|
||||
const auto &t_std = net_outputs.wide_from_device_euler.std;
|
||||
const auto &road_transform_trans_mean = net_outputs.road_transform.position_mean;
|
||||
const auto &road_transform_trans_std = net_outputs.road_transform.position_std;
|
||||
|
||||
auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry();
|
||||
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
|
||||
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
|
||||
posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_mean.z});
|
||||
posenetd.setRoadTransformTrans({road_transform_trans_mean.x, road_transform_trans_mean.y, road_transform_trans_mean.z});
|
||||
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
|
||||
posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
|
||||
posenetd.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)});
|
||||
posenetd.setRoadTransformTransStd({exp(road_transform_trans_std.x), exp(road_transform_trans_std.y), exp(road_transform_trans_std.z)});
|
||||
|
||||
posenetd.setTimestampEof(timestamp_eof);
|
||||
posenetd.setFrameId(vipc_frame_id);
|
||||
}
|
||||
@@ -1,257 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <memory>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/modeldata.h"
|
||||
#include "common/util.h"
|
||||
#include "selfdrive/modeld/models/commonmodel.h"
|
||||
#include "selfdrive/modeld/runners/run.h"
|
||||
|
||||
constexpr int FEATURE_LEN = 512;
|
||||
constexpr int HISTORY_BUFFER_LEN = 99;
|
||||
constexpr int DESIRE_LEN = 8;
|
||||
constexpr int DESIRE_PRED_LEN = 4;
|
||||
constexpr int TRAFFIC_CONVENTION_LEN = 2;
|
||||
constexpr int NAV_FEATURE_LEN = 256;
|
||||
constexpr int NAV_INSTRUCTION_LEN = 150;
|
||||
constexpr int DRIVING_STYLE_LEN = 12;
|
||||
constexpr int MODEL_FREQ = 20;
|
||||
|
||||
constexpr int DISENGAGE_LEN = 5;
|
||||
constexpr int BLINKER_LEN = 6;
|
||||
constexpr int META_STRIDE = 7;
|
||||
|
||||
constexpr int PLAN_MHP_N = 5;
|
||||
constexpr int LEAD_MHP_N = 2;
|
||||
constexpr int LEAD_TRAJ_LEN = 6;
|
||||
constexpr int LEAD_MHP_SELECTION = 3;
|
||||
// Padding to get output shape as multiple of 4
|
||||
constexpr int PAD_SIZE = 2;
|
||||
|
||||
constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
|
||||
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
|
||||
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
|
||||
|
||||
struct ModelOutputXYZ {
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3);
|
||||
|
||||
struct ModelOutputYZ {
|
||||
float y;
|
||||
float z;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
|
||||
|
||||
struct ModelOutputPlanElement {
|
||||
ModelOutputXYZ position;
|
||||
ModelOutputXYZ velocity;
|
||||
ModelOutputXYZ acceleration;
|
||||
ModelOutputXYZ rotation;
|
||||
ModelOutputXYZ rotation_rate;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5);
|
||||
|
||||
struct ModelOutputPlanPrediction {
|
||||
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> mean;
|
||||
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> std;
|
||||
float prob;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
|
||||
|
||||
struct ModelOutputPlans {
|
||||
std::array<ModelOutputPlanPrediction, PLAN_MHP_N> prediction;
|
||||
|
||||
constexpr const ModelOutputPlanPrediction &get_best_prediction() const {
|
||||
int max_idx = 0;
|
||||
for (int i = 1; i < prediction.size(); i++) {
|
||||
if (prediction[i].prob > prediction[max_idx].prob) {
|
||||
max_idx = i;
|
||||
}
|
||||
}
|
||||
return prediction[max_idx];
|
||||
}
|
||||
};
|
||||
static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
|
||||
|
||||
struct ModelOutputLinesXY {
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far;
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near;
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near;
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
|
||||
|
||||
struct ModelOutputLineProbVal {
|
||||
float val_deprecated;
|
||||
float val;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2);
|
||||
|
||||
struct ModelOutputLinesProb {
|
||||
ModelOutputLineProbVal left_far;
|
||||
ModelOutputLineProbVal left_near;
|
||||
ModelOutputLineProbVal right_near;
|
||||
ModelOutputLineProbVal right_far;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
|
||||
|
||||
struct ModelOutputLaneLines {
|
||||
ModelOutputLinesXY mean;
|
||||
ModelOutputLinesXY std;
|
||||
ModelOutputLinesProb prob;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
|
||||
|
||||
struct ModelOutputEdgessXY {
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left;
|
||||
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2);
|
||||
|
||||
struct ModelOutputRoadEdges {
|
||||
ModelOutputEdgessXY mean;
|
||||
ModelOutputEdgessXY std;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2));
|
||||
|
||||
struct ModelOutputLeadElement {
|
||||
float x;
|
||||
float y;
|
||||
float velocity;
|
||||
float acceleration;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4);
|
||||
|
||||
struct ModelOutputLeadPrediction {
|
||||
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> mean;
|
||||
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> std;
|
||||
std::array<float, LEAD_MHP_SELECTION> prob;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
|
||||
|
||||
struct ModelOutputLeads {
|
||||
std::array<ModelOutputLeadPrediction, LEAD_MHP_N> prediction;
|
||||
std::array<float, LEAD_MHP_SELECTION> prob;
|
||||
|
||||
constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const {
|
||||
int max_idx = 0;
|
||||
for (int i = 1; i < prediction.size(); i++) {
|
||||
if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
|
||||
max_idx = i;
|
||||
}
|
||||
}
|
||||
return prediction[max_idx];
|
||||
}
|
||||
};
|
||||
static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
|
||||
|
||||
|
||||
struct ModelOutputPose {
|
||||
ModelOutputXYZ velocity_mean;
|
||||
ModelOutputXYZ rotation_mean;
|
||||
ModelOutputXYZ velocity_std;
|
||||
ModelOutputXYZ rotation_std;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4);
|
||||
|
||||
struct ModelOutputWideFromDeviceEuler {
|
||||
ModelOutputXYZ mean;
|
||||
ModelOutputXYZ std;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2);
|
||||
|
||||
struct ModelOutputTemporalPose {
|
||||
ModelOutputXYZ velocity_mean;
|
||||
ModelOutputXYZ rotation_mean;
|
||||
ModelOutputXYZ velocity_std;
|
||||
ModelOutputXYZ rotation_std;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
|
||||
|
||||
struct ModelOutputRoadTransform {
|
||||
ModelOutputXYZ position_mean;
|
||||
ModelOutputXYZ rotation_mean;
|
||||
ModelOutputXYZ position_std;
|
||||
ModelOutputXYZ rotation_std;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputRoadTransform) == sizeof(ModelOutputXYZ)*4);
|
||||
|
||||
struct ModelOutputDisengageProb {
|
||||
float gas_disengage;
|
||||
float brake_disengage;
|
||||
float steer_override;
|
||||
float brake_3ms2;
|
||||
float brake_4ms2;
|
||||
float brake_5ms2;
|
||||
float gas_pressed;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7);
|
||||
|
||||
struct ModelOutputBlinkerProb {
|
||||
float left;
|
||||
float right;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2);
|
||||
|
||||
struct ModelOutputDesireProb {
|
||||
union {
|
||||
struct {
|
||||
float none;
|
||||
float turn_left;
|
||||
float turn_right;
|
||||
float lane_change_left;
|
||||
float lane_change_right;
|
||||
float keep_left;
|
||||
float keep_right;
|
||||
float null;
|
||||
};
|
||||
struct {
|
||||
std::array<float, DESIRE_LEN> array;
|
||||
};
|
||||
};
|
||||
};
|
||||
static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN);
|
||||
|
||||
struct ModelOutputMeta {
|
||||
ModelOutputDesireProb desire_state_prob;
|
||||
float engaged_prob;
|
||||
std::array<ModelOutputDisengageProb, DISENGAGE_LEN> disengage_prob;
|
||||
std::array<ModelOutputBlinkerProb, BLINKER_LEN> blinker_prob;
|
||||
std::array<ModelOutputDesireProb, DESIRE_PRED_LEN> desire_pred_prob;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
|
||||
|
||||
struct ModelOutputFeatures {
|
||||
std::array<float, FEATURE_LEN> feature;
|
||||
};
|
||||
static_assert(sizeof(ModelOutputFeatures) == (sizeof(float)*FEATURE_LEN));
|
||||
|
||||
struct ModelOutput {
|
||||
const ModelOutputPlans plans;
|
||||
const ModelOutputLaneLines lane_lines;
|
||||
const ModelOutputRoadEdges road_edges;
|
||||
const ModelOutputLeads leads;
|
||||
const ModelOutputMeta meta;
|
||||
const ModelOutputPose pose;
|
||||
const ModelOutputWideFromDeviceEuler wide_from_device_euler;
|
||||
const ModelOutputTemporalPose temporal_pose;
|
||||
const ModelOutputRoadTransform road_transform;
|
||||
};
|
||||
|
||||
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
|
||||
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + PAD_SIZE;
|
||||
|
||||
struct PublishState {
|
||||
std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
|
||||
std::array<float, 5> prev_brake_5ms2_probs = {};
|
||||
std::array<float, 3> prev_brake_3ms2_probs = {};
|
||||
};
|
||||
|
||||
void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
|
||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid);
|
||||
void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid);
|
||||
@@ -1,25 +0,0 @@
|
||||
# distutils: language = c++
|
||||
|
||||
from libcpp cimport bool
|
||||
from libc.stdint cimport uint32_t, uint64_t
|
||||
|
||||
cdef extern from "cereal/messaging/messaging.h":
|
||||
cdef cppclass MessageBuilder:
|
||||
size_t getSerializedSize()
|
||||
int serializeToBuffer(unsigned char *, size_t)
|
||||
|
||||
cdef extern from "selfdrive/modeld/models/driving.h":
|
||||
cdef int FEATURE_LEN
|
||||
cdef int HISTORY_BUFFER_LEN
|
||||
cdef int DESIRE_LEN
|
||||
cdef int TRAFFIC_CONVENTION_LEN
|
||||
cdef int DRIVING_STYLE_LEN
|
||||
cdef int NAV_FEATURE_LEN
|
||||
cdef int NAV_INSTRUCTION_LEN
|
||||
cdef int OUTPUT_SIZE
|
||||
cdef int NET_OUTPUT_SIZE
|
||||
cdef int MODEL_FREQ
|
||||
cdef struct PublishState: pass
|
||||
|
||||
void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool)
|
||||
void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool)
|
||||
@@ -1,52 +0,0 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii
|
||||
|
||||
import numpy as np
|
||||
cimport numpy as cnp
|
||||
from libcpp cimport bool
|
||||
from libc.string cimport memcpy
|
||||
from libc.stdint cimport uint32_t, uint64_t
|
||||
|
||||
from .commonmodel cimport mat3
|
||||
from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_HISTORY_BUFFER_LEN, DESIRE_LEN as CPP_DESIRE_LEN, \
|
||||
TRAFFIC_CONVENTION_LEN as CPP_TRAFFIC_CONVENTION_LEN, DRIVING_STYLE_LEN as CPP_DRIVING_STYLE_LEN, \
|
||||
NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
|
||||
OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
|
||||
from .driving cimport MessageBuilder, PublishState as cppPublishState
|
||||
from .driving cimport fill_model_msg, fill_pose_msg
|
||||
|
||||
FEATURE_LEN = CPP_FEATURE_LEN
|
||||
HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
|
||||
DESIRE_LEN = CPP_DESIRE_LEN
|
||||
TRAFFIC_CONVENTION_LEN = CPP_TRAFFIC_CONVENTION_LEN
|
||||
DRIVING_STYLE_LEN = CPP_DRIVING_STYLE_LEN
|
||||
NAV_FEATURE_LEN = CPP_NAV_FEATURE_LEN
|
||||
NAV_INSTRUCTION_LEN = CPP_NAV_INSTRUCTION_LEN
|
||||
OUTPUT_SIZE = CPP_OUTPUT_SIZE
|
||||
NET_OUTPUT_SIZE = CPP_NET_OUTPUT_SIZE
|
||||
MODEL_FREQ = CPP_MODEL_FREQ
|
||||
|
||||
cdef class PublishState:
|
||||
cdef cppPublishState state
|
||||
|
||||
def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
|
||||
uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid):
|
||||
cdef MessageBuilder msg
|
||||
fill_model_msg(msg, &model_outputs[0], ps.state, vipc_frame_id, vipc_frame_id_extra, frame_id, frame_drop,
|
||||
timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, valid)
|
||||
|
||||
output_size = msg.getSerializedSize()
|
||||
output_data = bytearray(output_size)
|
||||
cdef unsigned char * output_ptr = output_data
|
||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
|
||||
return bytes(output_data)
|
||||
|
||||
def create_pose_msg(float[:] model_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, bool valid):
|
||||
cdef MessageBuilder msg
|
||||
fill_pose_msg(msg, &model_outputs[0], vipc_frame_id, vipc_dropped_frames, timestamp_eof, valid)
|
||||
|
||||
output_size = msg.getSerializedSize()
|
||||
output_data = bytearray(output_size)
|
||||
cdef unsigned char * output_ptr = output_data
|
||||
assert msg.serializeToBuffer(output_ptr, output_size) > 0, "output buffer is too small to serialize"
|
||||
return bytes(output_data)
|
||||
@@ -13,13 +13,13 @@ from cereal.visionipc import VisionIpcClient, VisionStreamType
|
||||
from openpilot.system.swaglog import cloudlog
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import set_realtime_priority
|
||||
from openpilot.selfdrive.modeld.constants import IDX_N
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
|
||||
|
||||
NAV_INPUT_SIZE = 256*256
|
||||
NAV_FEATURE_LEN = 256
|
||||
NAV_DESIRE_LEN = 32
|
||||
NAV_OUTPUT_SIZE = 2*2*IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
|
||||
NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN
|
||||
MODEL_PATHS = {
|
||||
ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc',
|
||||
ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'}
|
||||
@@ -31,8 +31,8 @@ class NavModelOutputXY(ctypes.Structure):
|
||||
|
||||
class NavModelOutputPlan(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("mean", NavModelOutputXY*IDX_N),
|
||||
("std", NavModelOutputXY*IDX_N)]
|
||||
("mean", NavModelOutputXY*ModelConstants.IDX_N),
|
||||
("std", NavModelOutputXY*ModelConstants.IDX_N)]
|
||||
|
||||
class NavModelResult(ctypes.Structure):
|
||||
_fields_ = [
|
||||
|
||||
@@ -0,0 +1,100 @@
|
||||
import numpy as np
|
||||
from typing import Dict
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
def sigmoid(x):
|
||||
return 1. / (1. + np.exp(-x))
|
||||
|
||||
def softmax(x, axis=-1):
|
||||
x -= np.max(x, axis=axis, keepdims=True)
|
||||
if x.dtype == np.float32 or x.dtype == np.float64:
|
||||
np.exp(x, out=x)
|
||||
else:
|
||||
x = np.exp(x)
|
||||
x /= np.sum(x, axis=axis, keepdims=True)
|
||||
return x
|
||||
|
||||
class Parser:
|
||||
def __init__(self, ignore_missing=False):
|
||||
self.ignore_missing = ignore_missing
|
||||
|
||||
def check_missing(self, outs, name):
|
||||
if name not in outs and not self.ignore_missing:
|
||||
raise ValueError(f"Missing output {name}")
|
||||
return name not in outs
|
||||
|
||||
def parse_categorical_crossentropy(self, name, outs, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
if out_shape is not None:
|
||||
raw = raw.reshape((raw.shape[0],) + out_shape)
|
||||
outs[name] = softmax(raw, axis=-1)
|
||||
|
||||
def parse_binary_crossentropy(self, name, outs):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
outs[name] = sigmoid(raw)
|
||||
|
||||
def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
|
||||
if self.check_missing(outs, name):
|
||||
return
|
||||
raw = outs[name]
|
||||
raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
|
||||
|
||||
pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2]
|
||||
n_values = (raw.shape[2] - out_N)//2
|
||||
pred_mu = raw[:,:,:n_values]
|
||||
pred_std = np.exp(raw[:,:,n_values: 2*n_values])
|
||||
|
||||
if in_N > 1:
|
||||
weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
|
||||
for i in range(out_N):
|
||||
weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
|
||||
|
||||
if out_N == 1:
|
||||
for fidx in range(weights.shape[0]):
|
||||
idxs = np.argsort(weights[fidx][:,0])[::-1]
|
||||
weights[fidx] = weights[fidx][idxs]
|
||||
pred_mu[fidx] = pred_mu[fidx][idxs]
|
||||
pred_std[fidx] = pred_std[fidx][idxs]
|
||||
full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
|
||||
outs[name + '_weights'] = weights
|
||||
outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
|
||||
outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
|
||||
|
||||
pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
|
||||
for fidx in range(weights.shape[0]):
|
||||
for hidx in range(out_N):
|
||||
idxs = np.argsort(weights[fidx,:,hidx])[::-1]
|
||||
pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
|
||||
pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
|
||||
else:
|
||||
pred_mu_final = pred_mu
|
||||
pred_std_final = pred_std
|
||||
|
||||
if out_N > 1:
|
||||
final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
|
||||
else:
|
||||
final_shape = tuple([raw.shape[0],] + list(out_shape))
|
||||
outs[name] = pred_mu_final.reshape(final_shape)
|
||||
outs[name + '_stds'] = pred_std_final.reshape(final_shape)
|
||||
|
||||
def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]:
|
||||
self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
|
||||
self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
|
||||
self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
|
||||
self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
|
||||
self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
|
||||
out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
|
||||
for k in ['lead_prob', 'lane_lines_prob', 'meta']:
|
||||
self.parse_binary_crossentropy(k, outs)
|
||||
self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
|
||||
self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
|
||||
return outs
|
||||
@@ -1,32 +0,0 @@
|
||||
import struct
|
||||
import json
|
||||
|
||||
def load_thneed(fn):
|
||||
with open(fn, "rb") as f:
|
||||
json_len = struct.unpack("I", f.read(4))[0]
|
||||
jdat = json.loads(f.read(json_len).decode('latin_1'))
|
||||
weights = f.read()
|
||||
ptr = 0
|
||||
for o in jdat['objects']:
|
||||
if o['needs_load']:
|
||||
nptr = ptr + o['size']
|
||||
o['data'] = weights[ptr:nptr]
|
||||
ptr = nptr
|
||||
for o in jdat['binaries']:
|
||||
nptr = ptr + o['length']
|
||||
o['data'] = weights[ptr:nptr]
|
||||
ptr = nptr
|
||||
return jdat
|
||||
|
||||
def save_thneed(jdat, fn):
|
||||
new_weights = []
|
||||
for o in jdat['objects'] + jdat['binaries']:
|
||||
if 'data' in o:
|
||||
new_weights.append(o['data'])
|
||||
del o['data']
|
||||
new_weights_bytes = b''.join(new_weights)
|
||||
with open(fn, "wb") as f:
|
||||
j = json.dumps(jdat, ensure_ascii=False).encode('latin_1')
|
||||
f.write(struct.pack("I", len(j)))
|
||||
f.write(j)
|
||||
f.write(new_weights_bytes)
|
||||
@@ -6,7 +6,7 @@ from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.realtime import Ratekeeper, DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.modeld.constants import T_IDXS
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
@@ -100,13 +100,13 @@ class Plant:
|
||||
# this is to ensure lead policy is effective when model
|
||||
# does not predict slowdown in e2e mode
|
||||
position = log.XYZTData.new_message()
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(T_IDXS)]
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
model.modelV2.position = position
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(T_IDXS)]
|
||||
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(T_IDXS)]
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
|
||||
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
|
||||
|
||||
@@ -1 +1 @@
|
||||
f851c7e7f90eff828a59444d20fac5df8cd7ae0c
|
||||
0e0f55cf3bb2cf79b44adf190e6387a83deb6646
|
||||
|
||||
@@ -37,7 +37,7 @@ PROCS = {
|
||||
"selfdrive.locationd.paramsd": 9.0,
|
||||
"./sensord": 7.0,
|
||||
"selfdrive.controls.radard": 4.5,
|
||||
"selfdrive.modeld.modeld": 8.0,
|
||||
"selfdrive.modeld.modeld": 13.0,
|
||||
"selfdrive.modeld.dmonitoringmodeld": 8.0,
|
||||
"selfdrive.modeld.navmodeld": 1.0,
|
||||
"selfdrive.thermald.thermald": 3.87,
|
||||
|
||||
@@ -28,7 +28,7 @@ class Proc:
|
||||
|
||||
PROCS = [
|
||||
Proc('camerad', 2.1, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']),
|
||||
Proc('modeld', 0.93, atol=0.2, msgs=['modelV2']),
|
||||
Proc('modeld', 1.0, atol=0.2, msgs=['modelV2']),
|
||||
Proc('dmonitoringmodeld', 0.4, msgs=['driverStateV2']),
|
||||
Proc('encoderd', 0.23, msgs=[]),
|
||||
Proc('mapsd', 0.05, msgs=['mapRenderState']),
|
||||
|
||||
@@ -31,6 +31,7 @@ COPY ./panda ${OPENPILOT_PATH}/panda
|
||||
COPY ./selfdrive ${OPENPILOT_PATH}/selfdrive
|
||||
COPY ./system ${OPENPILOT_PATH}/system
|
||||
COPY ./tools ${OPENPILOT_PATH}/tools
|
||||
COPY ./release ${OPENPILOT_PATH}/release
|
||||
|
||||
RUN --mount=type=bind,source=.ci_cache/scons_cache,target=/tmp/scons_cache,rw scons -j$(nproc) --cache-readonly
|
||||
|
||||
|
||||
Reference in New Issue
Block a user