remove old model packet (#19769)
* remove publisher * move to V2 * radard new model * fix plant * change packety * need hack here too * change to new * this has been wrong all along * no more model msg * subscribe to new model * not needed anymore * make work * need to ignore that too * should pass tests, needs car test * fix process replay * no more poly * update refs Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> old-commit-hash: 124100d0fa42485edaf86cb25f77f27e8917eed1
This commit is contained in:
@@ -53,7 +53,7 @@ class Controls:
|
||||
self.sm = sm
|
||||
if self.sm is None:
|
||||
ignore = ['ubloxRaw', 'frontFrame'] if SIMULATION else None
|
||||
self.sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'ubloxRaw',
|
||||
self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw',
|
||||
'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman',
|
||||
'frame', 'frontFrame'], ignore_alive=ignore)
|
||||
|
||||
@@ -237,7 +237,7 @@ class Controls:
|
||||
self.events.add(EventName.noGps)
|
||||
if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL):
|
||||
self.events.add(EventName.cameraMalfunction)
|
||||
if self.sm['model'].frameDropPerc > 20:
|
||||
if self.sm['modelV2'].frameDropPerc > 20:
|
||||
self.events.add(EventName.modeldLagging)
|
||||
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
@@ -433,12 +433,12 @@ class Controls:
|
||||
ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \
|
||||
and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED
|
||||
|
||||
meta = self.sm['model'].meta
|
||||
meta = self.sm['modelV2'].meta
|
||||
if len(meta.desirePrediction) and ldw_allowed:
|
||||
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
|
||||
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
|
||||
l_lane_close = left_lane_visible and (self.sm['model'].leftLane.poly[3] < (1.08 - CAMERA_OFFSET))
|
||||
r_lane_close = right_lane_visible and (self.sm['model'].rightLane.poly[3] > -(1.08 + CAMERA_OFFSET))
|
||||
l_lane_close = left_lane_visible and (self.sm['modelV2'].laneLines[1].y[0] > -(1.08 + CAMERA_OFFSET))
|
||||
r_lane_close = right_lane_visible and (self.sm['modelV2'].laneLines[2].y[0] < (1.08 - CAMERA_OFFSET))
|
||||
|
||||
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
|
||||
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
|
||||
|
||||
@@ -132,11 +132,11 @@ class Cluster():
|
||||
|
||||
def get_RadarState_from_vision(self, lead_msg, v_ego):
|
||||
return {
|
||||
"dRel": float(lead_msg.dist - RADAR_TO_CAMERA),
|
||||
"yRel": float(lead_msg.relY),
|
||||
"vRel": float(lead_msg.relVel),
|
||||
"vLead": float(v_ego + lead_msg.relVel),
|
||||
"vLeadK": float(v_ego + lead_msg.relVel),
|
||||
"dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA),
|
||||
"yRel": float(-lead_msg.xyva[1]),
|
||||
"vRel": float(lead_msg.xyva[2]),
|
||||
"vLead": float(v_ego + lead_msg.xyva[2]),
|
||||
"vLeadK": float(v_ego + lead_msg.xyva[2]),
|
||||
"aLeadK": float(0),
|
||||
"aLeadTau": _LEAD_ACCEL_TAU,
|
||||
"fcw": False,
|
||||
|
||||
@@ -37,12 +37,12 @@ def laplacian_cdf(x, mu, b):
|
||||
|
||||
def match_vision_to_cluster(v_ego, lead, clusters):
|
||||
# match vision point to best statistical cluster match
|
||||
offset_vision_dist = lead.dist - RADAR_TO_CAMERA
|
||||
offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA
|
||||
|
||||
def prob(c):
|
||||
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std)
|
||||
prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd)
|
||||
prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd)
|
||||
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0])
|
||||
prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
|
||||
prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
|
||||
|
||||
# This is isn't exactly right, but good heuristic
|
||||
return prob_d * prob_y * prob_v
|
||||
@@ -52,7 +52,7 @@ def match_vision_to_cluster(v_ego, lead, clusters):
|
||||
# if no 'sane' match is found return -1
|
||||
# stationary radar points can be false positives
|
||||
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
|
||||
vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 3)
|
||||
vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3)
|
||||
if dist_sane and vel_sane:
|
||||
return cluster
|
||||
else:
|
||||
@@ -103,7 +103,7 @@ class RadarD():
|
||||
if sm.updated['controlsState']:
|
||||
self.v_ego = sm['controlsState'].vEgo
|
||||
self.v_ego_hist.append(self.v_ego)
|
||||
if sm.updated['model']:
|
||||
if sm.updated['modelV2']:
|
||||
self.ready = True
|
||||
|
||||
ar_pts = {}
|
||||
@@ -159,14 +159,15 @@ class RadarD():
|
||||
dat = messaging.new_message('radarState')
|
||||
dat.valid = sm.all_alive_and_valid()
|
||||
radarState = dat.radarState
|
||||
radarState.mdMonoTime = sm.logMonoTime['model']
|
||||
radarState.mdMonoTime = sm.logMonoTime['modelV2']
|
||||
radarState.canMonoTimes = list(rr.canMonoTimes)
|
||||
radarState.radarErrors = list(rr.errors)
|
||||
radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
|
||||
|
||||
if enable_lead:
|
||||
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True)
|
||||
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False)
|
||||
if len(sm['modelV2'].leads) > 1:
|
||||
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True)
|
||||
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False)
|
||||
return dat
|
||||
|
||||
|
||||
@@ -187,7 +188,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
|
||||
if can_sock is None:
|
||||
can_sock = messaging.sub_sock('can')
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['model', 'controlsState'])
|
||||
sm = messaging.SubMaster(['modelV2', 'controlsState'])
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['radarState', 'liveTracks'])
|
||||
|
||||
|
||||
@@ -106,7 +106,7 @@ int main(int argc, char **argv) {
|
||||
assert(err == 0);
|
||||
|
||||
// messaging
|
||||
PubMaster pm({"modelV2", "model", "cameraOdometry"});
|
||||
PubMaster pm({"modelV2", "cameraOdometry"});
|
||||
SubMaster sm({"pathPlan", "frame"});
|
||||
|
||||
// cl init
|
||||
|
||||
@@ -10,8 +10,6 @@
|
||||
#include "driving.h"
|
||||
#include "clutil.h"
|
||||
|
||||
constexpr int MODEL_PATH_DISTANCE = 192;
|
||||
constexpr int POLYFIT_DEGREE = 4;
|
||||
constexpr int DESIRE_PRED_SIZE = 32;
|
||||
constexpr int OTHER_META_SIZE = 4;
|
||||
|
||||
@@ -32,7 +30,6 @@ constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
|
||||
|
||||
constexpr int POSE_SIZE = 12;
|
||||
|
||||
constexpr int MIN_VALID_LEN = 10;
|
||||
constexpr int PLAN_IDX = 0;
|
||||
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
|
||||
constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
|
||||
@@ -51,8 +48,6 @@ constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
|
||||
|
||||
// #define DUMP_YUV
|
||||
|
||||
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
|
||||
|
||||
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
|
||||
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
|
||||
s->input_frames = std::make_unique<float[]>(MODEL_FRAME_SIZE * 2);
|
||||
@@ -75,13 +70,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
|
||||
s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
|
||||
#endif
|
||||
|
||||
// Build Vandermonde matrix
|
||||
for(int i = 0; i < TRAJECTORY_SIZE; i++) {
|
||||
for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
|
||||
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
|
||||
}
|
||||
}
|
||||
|
||||
s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
|
||||
}
|
||||
|
||||
@@ -136,31 +124,6 @@ void model_free(ModelState* s) {
|
||||
CL_CHECK(clReleaseCommandQueue(s->q));
|
||||
}
|
||||
|
||||
void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
|
||||
// References to inputs
|
||||
Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1> > pts(in_pts, valid_len);
|
||||
Eigen::Map<Eigen::Matrix<float, Eigen::Dynamic, 1> > std(in_stds, valid_len);
|
||||
Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> > p(out, POLYFIT_DEGREE - 1);
|
||||
|
||||
float y0 = pts[0];
|
||||
pts = pts.array() - y0;
|
||||
|
||||
// Build Least Squares equations
|
||||
Eigen::Matrix<float, Eigen::Dynamic, POLYFIT_DEGREE - 1> lhs = vander.topRows(valid_len).array().colwise() / std.array();
|
||||
Eigen::Matrix<float, Eigen::Dynamic, 1> rhs = pts.array() / std.array();
|
||||
|
||||
// Improve numerical stability
|
||||
Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
|
||||
lhs = lhs * scale.asDiagonal();
|
||||
|
||||
// Solve inplace
|
||||
p = lhs.colPivHouseholderQr().solve(rhs);
|
||||
|
||||
// Apply scale to output
|
||||
p = p.transpose() * scale.asDiagonal();
|
||||
out[3] = y0;
|
||||
}
|
||||
|
||||
static const float *get_best_data(const float *data, int size, int group_size, int offset) {
|
||||
int max_idx = 0;
|
||||
for (int i = 1; i < size; i++) {
|
||||
@@ -180,30 +143,6 @@ static const float *get_lead_data(const float *lead, int t_offset) {
|
||||
return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
|
||||
}
|
||||
|
||||
void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob,
|
||||
float valid_len, int valid_len_idx, int ll_idx) {
|
||||
float points[TRAJECTORY_SIZE] = {};
|
||||
float stds[TRAJECTORY_SIZE] = {};
|
||||
float poly[POLYFIT_DEGREE] = {};
|
||||
|
||||
for (int i=0; i<TRAJECTORY_SIZE; i++) {
|
||||
// negative sign because mpc has left positive
|
||||
if (ll_idx == 0) {
|
||||
points[i] = -data[30 * i + 16];
|
||||
stds[i] = exp(data[30 * (33 + i) + 16]);
|
||||
} else {
|
||||
points[i] = -data[2 * 33 * ll_idx + 2 * i];
|
||||
stds[i] = exp(data[2 * 33 * (4 + ll_idx) + 2 * i]);
|
||||
}
|
||||
}
|
||||
const float std = stds[0];
|
||||
poly_fit(points, stds, poly, valid_len_idx);
|
||||
|
||||
path.setPoly(poly);
|
||||
path.setProb(prob);
|
||||
path.setStd(std);
|
||||
path.setValidLen(valid_len);
|
||||
}
|
||||
|
||||
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
|
||||
const float *data = get_lead_data(lead_data, t_offset);
|
||||
@@ -212,27 +151,13 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *le
|
||||
float xyva_arr[LEAD_MHP_VALS];
|
||||
float xyva_stds_arr[LEAD_MHP_VALS];
|
||||
for (int i=0; i<LEAD_MHP_VALS; i++) {
|
||||
xyva_arr[i] = data[LEAD_MHP_VALS + i];
|
||||
xyva_arr[i] = data[i];
|
||||
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
|
||||
}
|
||||
lead.setXyva(xyva_arr);
|
||||
lead.setXyvaStd(xyva_stds_arr);
|
||||
}
|
||||
|
||||
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float *lead_data, const float *prob, int t_offset) {
|
||||
const float *data = get_lead_data(lead_data, t_offset);
|
||||
lead.setProb(sigmoid(prob[t_offset]));
|
||||
lead.setDist(data[0]);
|
||||
lead.setStd(exp(data[LEAD_MHP_VALS]));
|
||||
// TODO make all msgs same format
|
||||
lead.setRelY(-data[1]);
|
||||
lead.setRelYStd(exp(data[LEAD_MHP_VALS + 1]));
|
||||
lead.setRelVel(data[2]);
|
||||
lead.setRelVelStd(exp(data[LEAD_MHP_VALS + 2]));
|
||||
lead.setRelA(data[3]);
|
||||
lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
|
||||
}
|
||||
|
||||
template <class MetaBuilder>
|
||||
void fill_meta(MetaBuilder meta, const float *meta_data) {
|
||||
float desire_state_softmax[DESIRE_LEN];
|
||||
@@ -332,54 +257,22 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
|
||||
}
|
||||
}
|
||||
|
||||
void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) {
|
||||
// Find the distribution that corresponds to the most probable plan
|
||||
const float *best_plan = get_plan_data(net_outputs.plan);
|
||||
// x pos at 10s is a good valid_len
|
||||
float valid_len = 0;
|
||||
for (int i=1; i<TRAJECTORY_SIZE; i++) {
|
||||
if (const float len = best_plan[30*i]; len >= valid_len){
|
||||
valid_len = len;
|
||||
}
|
||||
}
|
||||
// clamp to 10 and MODEL_PATH_DISTANCE
|
||||
valid_len = fmin(MODEL_PATH_DISTANCE, fmax(MIN_VALID_LEN, valid_len));
|
||||
int valid_len_idx = 0;
|
||||
for (int i=1; i<TRAJECTORY_SIZE; i++) {
|
||||
if (valid_len >= X_IDXS[valid_len_idx]){
|
||||
valid_len_idx = i;
|
||||
}
|
||||
}
|
||||
fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0);
|
||||
fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1);
|
||||
fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2);
|
||||
|
||||
fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0);
|
||||
fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1);
|
||||
|
||||
fill_meta(framed.initMeta(), net_outputs.meta);
|
||||
}
|
||||
|
||||
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
|
||||
const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
|
||||
float model_execution_time) {
|
||||
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||
auto do_publish = [&](auto init_model_func, const char *pub_name) {
|
||||
MessageBuilder msg;
|
||||
auto framed = (msg.initEvent().*(init_model_func))();
|
||||
framed.setFrameId(vipc_frame_id);
|
||||
framed.setFrameAge(frame_age);
|
||||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
framed.setModelExecutionTime(model_execution_time);
|
||||
if (send_raw_pred) {
|
||||
framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float)));
|
||||
}
|
||||
fill_model(framed, net_outputs);
|
||||
pm.send(pub_name, msg);
|
||||
};
|
||||
do_publish(&cereal::Event::Builder::initModel, "model");
|
||||
do_publish(&cereal::Event::Builder::initModelV2, "modelV2");
|
||||
MessageBuilder msg;
|
||||
auto framed = msg.initEvent().initModelV2();
|
||||
framed.setFrameId(vipc_frame_id);
|
||||
framed.setFrameAge(frame_age);
|
||||
framed.setFrameDropPerc(frame_drop * 100);
|
||||
framed.setTimestampEof(timestamp_eof);
|
||||
framed.setModelExecutionTime(model_execution_time);
|
||||
if (send_raw_pred) {
|
||||
framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float)));
|
||||
}
|
||||
fill_model(framed, net_outputs);
|
||||
pm.send("modelV2", msg);
|
||||
}
|
||||
|
||||
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
|
||||
|
||||
@@ -12,7 +12,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
||||
pm = messaging.PubMaster(['dMonitoringState'])
|
||||
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'model'], poll=['driverState'])
|
||||
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState'])
|
||||
|
||||
driver_status = DriverStatus(rhd=Params().get("IsRHD") == b"1")
|
||||
|
||||
@@ -44,8 +44,8 @@ def dmonitoringd_thread(sm=None, pm=None):
|
||||
driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill)
|
||||
v_cruise_last = v_cruise
|
||||
|
||||
if sm.updated['model']:
|
||||
driver_status.set_policy(sm['model'])
|
||||
if sm.updated['modelV2']:
|
||||
driver_status.set_policy(sm['modelV2'])
|
||||
|
||||
# Get data from dmonitoringmodeld
|
||||
events = Events()
|
||||
|
||||
@@ -8,7 +8,7 @@ import numpy as np
|
||||
|
||||
from opendbc import DBC_PATH
|
||||
|
||||
from cereal import car
|
||||
from cereal import car, log
|
||||
from common.realtime import Ratekeeper
|
||||
from selfdrive.config import Conversions as CV
|
||||
import cereal.messaging as messaging
|
||||
@@ -111,10 +111,10 @@ class Plant():
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
|
||||
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw', 'modelV2'])
|
||||
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw'])
|
||||
Plant.logcan = messaging.pub_sock('can')
|
||||
Plant.sendcan = messaging.sub_sock('sendcan')
|
||||
Plant.model = messaging.pub_sock('model')
|
||||
Plant.model = messaging.pub_sock('modelV2')
|
||||
Plant.live_params = messaging.pub_sock('liveParameters')
|
||||
Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
|
||||
Plant.health = messaging.pub_sock('health')
|
||||
@@ -392,13 +392,9 @@ class Plant():
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
if publish_model and self.frame % 5 == 0:
|
||||
md = messaging.new_message('model')
|
||||
md = messaging.new_message('modelV2')
|
||||
cal = messaging.new_message('liveCalibration')
|
||||
md.model.frameId = 0
|
||||
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
|
||||
x.points = [0.0]*50
|
||||
x.prob = 1.0
|
||||
x.std = 1.0
|
||||
md.modelV2.frameId = 0
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0., distance_lead - distance)
|
||||
@@ -409,15 +405,11 @@ class Plant():
|
||||
v_rel = 0.
|
||||
prob = 0.0
|
||||
|
||||
md.model.lead.dist = float(d_rel)
|
||||
md.model.lead.prob = prob
|
||||
md.model.lead.relY = 0.0
|
||||
md.model.lead.relYStd = 1.
|
||||
md.model.lead.relVel = float(v_rel)
|
||||
md.model.lead.relVelStd = 1.
|
||||
md.model.lead.relA = 0.0
|
||||
md.model.lead.relAStd = 10.
|
||||
md.model.lead.std = 1.0
|
||||
lead = log.ModelDataV2.LeadDataV2.new_message()
|
||||
lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0]
|
||||
lead.xyvaStd = [1.0, 1.0, 1.0, 1.0]
|
||||
lead.prob = prob
|
||||
md.modelV2.leads = [lead, lead]
|
||||
|
||||
cal.liveCalibration.calStatus = 1
|
||||
cal.liveCalibration.calPerc = 100
|
||||
|
||||
@@ -35,7 +35,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
|
||||
spinner.update("starting model replay")
|
||||
|
||||
pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan'])
|
||||
sm = messaging.SubMaster(['model', 'modelV2'])
|
||||
sm = messaging.SubMaster(['modelV2'])
|
||||
|
||||
# TODO: add dmonitoringmodeld
|
||||
print("preparing procs")
|
||||
@@ -74,7 +74,6 @@ def camera_replay(lr, fr, desire=None, calib=None):
|
||||
|
||||
pm.send(msg.which(), f)
|
||||
with Timeout(seconds=15):
|
||||
log_msgs.append(messaging.recv_one(sm.sock['model']))
|
||||
log_msgs.append(messaging.recv_one(sm.sock['modelV2']))
|
||||
|
||||
spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count))
|
||||
@@ -109,8 +108,9 @@ if __name__ == "__main__":
|
||||
log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit)
|
||||
cmp_log = LogReader(BASE_URL + log_fn)
|
||||
|
||||
ignore = ['logMonoTime', 'valid', 'model.frameDropPerc', 'model.modelExecutionTime',
|
||||
'modelV2.frameDropPerc', 'modelV2.modelExecutionTime']
|
||||
ignore = ['logMonoTime', 'valid',
|
||||
'modelV2.frameDropPerc',
|
||||
'modelV2.modelExecutionTime']
|
||||
results: Any = {TEST_ROUTE: {}}
|
||||
results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, ignore_fields=ignore)
|
||||
diff1, diff2, failed = format_diff(results, ref_commit)
|
||||
|
||||
@@ -222,7 +222,7 @@ CONFIGS = [
|
||||
pub_sub={
|
||||
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
|
||||
"thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [],
|
||||
"model": [], "frontFrame": [], "frame": [], "ubloxRaw": [],
|
||||
"modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [],
|
||||
},
|
||||
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
|
||||
init_callback=fingerprint,
|
||||
@@ -233,7 +233,7 @@ CONFIGS = [
|
||||
proc_name="radard",
|
||||
pub_sub={
|
||||
"can": ["radarState", "liveTracks"],
|
||||
"liveParameters": [], "controlsState": [], "model": [],
|
||||
"liveParameters": [], "controlsState": [], "modelV2": [],
|
||||
},
|
||||
ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
|
||||
init_callback=get_car_params,
|
||||
@@ -266,7 +266,7 @@ CONFIGS = [
|
||||
proc_name="dmonitoringd",
|
||||
pub_sub={
|
||||
"driverState": ["dMonitoringState"],
|
||||
"liveCalibration": [], "carState": [], "model": [], "controlsState": [],
|
||||
"liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
|
||||
},
|
||||
ignore=["logMonoTime", "valid"],
|
||||
init_callback=get_car_params,
|
||||
|
||||
@@ -1 +1 @@
|
||||
3686b117b61bfe6ad49bdef435b9112869bbf7e0
|
||||
0b75e9c24662151100e77057ff7d1ffcf55d841a
|
||||
@@ -158,9 +158,6 @@ if __name__ == "__main__":
|
||||
if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \
|
||||
(not procs_whitelisted and cfg.proc_name in args.blacklist_procs):
|
||||
continue
|
||||
# TODO remove this hack
|
||||
if cfg.proc_name == 'plannerd' and car_brand in ["GM", "SUBARU", "VOLKSWAGEN", "NISSAN"]:
|
||||
continue
|
||||
|
||||
cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit))
|
||||
results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs)
|
||||
|
||||
Reference in New Issue
Block a user