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 committesting-closetd8807c8348. * 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:cad17b1255
YassineYousfi
2 years ago
committed by
GitHub
parent
f89f1ce4ab
commit
d18f185115
25 changed files with 478 additions and 768 deletions
@ -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() |
||||
@ -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}') |
||||
@ -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) |
||||
@ -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) |
||||
@ -1 +1 @@ |
||||
f851c7e7f90eff828a59444d20fac5df8cd7ae0c |
||||
0e0f55cf3bb2cf79b44adf190e6387a83deb6646 |
||||
|
||||
Loading…
Reference in new issue