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

remove old model packet (#19769)

Browse Source 
* 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: 124100d0fa
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent eb34e08426
commit 5c2c6691b2
11 changed files with 56 additions and 173 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      selfdrive/controls/controlsd.py
  2. 10
      selfdrive/controls/lib/radar_helpers.py
  3. 21
      selfdrive/controls/radard.py
  4. 2
      selfdrive/modeld/modeld.cc
  5. 133
      selfdrive/modeld/models/driving.cc
  6. 6
      selfdrive/monitoring/dmonitoringd.py
  7. 28
      selfdrive/test/longitudinal_maneuvers/plant.py
  8. 8
      selfdrive/test/process_replay/camera_replay.py
  9. 6
      selfdrive/test/process_replay/process_replay.py
  10. 2
      selfdrive/test/process_replay/ref_commit
  11. 3
      selfdrive/test/process_replay/test_processes.py

10
selfdrive/controls/controlsd.py
Unescape View File

@ -53,7 +53,7 @@ class Controls:
self.sm = sm self.sm = sm
if self.sm is None: if self.sm is None:
ignore = ['ubloxRaw', 'frontFrame'] if SIMULATION else 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', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman',
'frame', 'frontFrame'], ignore_alive=ignore) 'frame', 'frontFrame'], ignore_alive=ignore)
@ -237,7 +237,7 @@ class Controls:
self.events.add(EventName.noGps) self.events.add(EventName.noGps)
if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL): if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.cameraMalfunction) self.events.add(EventName.cameraMalfunction)
if self.sm['model'].frameDropPerc > 20: if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging) self.events.add(EventName.modeldLagging)
# Only allow engagement with brake pressed when stopped behind another stopped car # 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 \ 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 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: if len(meta.desirePrediction) and ldw_allowed:
l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1] l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 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)) 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['model'].rightLane.poly[3] > -(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.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) CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)

10
selfdrive/controls/lib/radar_helpers.py
Unescape View File

@ -132,11 +132,11 @@ class Cluster():
def get_RadarState_from_vision(self, lead_msg, v_ego): def get_RadarState_from_vision(self, lead_msg, v_ego):
return { return {
"dRel": float(lead_msg.dist - RADAR_TO_CAMERA), "dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA),
"yRel": float(lead_msg.relY), "yRel": float(-lead_msg.xyva[1]),
"vRel": float(lead_msg.relVel), "vRel": float(lead_msg.xyva[2]),
"vLead": float(v_ego + lead_msg.relVel), "vLead": float(v_ego + lead_msg.xyva[2]),
"vLeadK": float(v_ego + lead_msg.relVel), "vLeadK": float(v_ego + lead_msg.xyva[2]),
"aLeadK": float(0), "aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU, "aLeadTau": _LEAD_ACCEL_TAU,
"fcw": False, "fcw": False,

21
selfdrive/controls/radard.py
Unescape View File

@ -37,12 +37,12 @@ def laplacian_cdf(x, mu, b):
def match_vision_to_cluster(v_ego, lead, clusters): def match_vision_to_cluster(v_ego, lead, clusters):
# match vision point to best statistical cluster match # 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): def prob(c):
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0])
prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
# This is isn't exactly right, but good heuristic # This is isn't exactly right, but good heuristic
return prob_d * prob_y * prob_v 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 # if no 'sane' match is found return -1
# stationary radar points can be false positives # stationary radar points can be false positives
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) 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: if dist_sane and vel_sane:
return cluster return cluster
else: else:
@ -103,7 +103,7 @@ class RadarD():
if sm.updated['controlsState']: if sm.updated['controlsState']:
self.v_ego = sm['controlsState'].vEgo self.v_ego = sm['controlsState'].vEgo
self.v_ego_hist.append(self.v_ego) self.v_ego_hist.append(self.v_ego)
if sm.updated['model']: if sm.updated['modelV2']:
self.ready = True self.ready = True
ar_pts = {} ar_pts = {}
@ -159,14 +159,15 @@ class RadarD():
dat = messaging.new_message('radarState') dat = messaging.new_message('radarState')
dat.valid = sm.all_alive_and_valid() dat.valid = sm.all_alive_and_valid()
radarState = dat.radarState radarState = dat.radarState
radarState.mdMonoTime = sm.logMonoTime['model'] radarState.mdMonoTime = sm.logMonoTime['modelV2']
radarState.canMonoTimes = list(rr.canMonoTimes) radarState.canMonoTimes = list(rr.canMonoTimes)
radarState.radarErrors = list(rr.errors) radarState.radarErrors = list(rr.errors)
radarState.controlsStateMonoTime = sm.logMonoTime['controlsState'] radarState.controlsStateMonoTime = sm.logMonoTime['controlsState']
if enable_lead: if enable_lead:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['model'].lead, low_speed_override=True) if len(sm['modelV2'].leads) > 1:
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['model'].leadFuture, low_speed_override=False) 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 return dat
@ -187,7 +188,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
if can_sock is None: if can_sock is None:
can_sock = messaging.sub_sock('can') can_sock = messaging.sub_sock('can')
if sm is None: if sm is None:
sm = messaging.SubMaster(['model', 'controlsState']) sm = messaging.SubMaster(['modelV2', 'controlsState'])
if pm is None: if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks']) pm = messaging.PubMaster(['radarState', 'liveTracks'])

2
selfdrive/modeld/modeld.cc
Unescape View File

@ -106,7 +106,7 @@ int main(int argc, char **argv) {
assert(err == 0); assert(err == 0);
// messaging // messaging
PubMaster pm({"modelV2", "model", "cameraOdometry"}); PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"pathPlan", "frame"}); SubMaster sm({"pathPlan", "frame"});
// cl init // cl init

133
selfdrive/modeld/models/driving.cc
Unescape View File

@ -10,8 +10,6 @@
#include "driving.h" #include "driving.h"
#include "clutil.h" #include "clutil.h"
constexpr int MODEL_PATH_DISTANCE = 192;
constexpr int POLYFIT_DEGREE = 4;
constexpr int DESIRE_PRED_SIZE = 32; constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 4; 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 POSE_SIZE = 12;
constexpr int MIN_VALID_LEN = 10;
constexpr int PLAN_IDX = 0; constexpr int PLAN_IDX = 0;
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE; constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33; 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 // #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) { void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
s->input_frames = std::make_unique<float[]>(MODEL_FRAME_SIZE * 2); 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); s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
#endif #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)); 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)); 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) { static const float *get_best_data(const float *data, int size, int group_size, int offset) {
int max_idx = 0; int max_idx = 0;
for (int i = 1; i < size; i++) { 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); 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) { 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); 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_arr[LEAD_MHP_VALS];
float xyva_stds_arr[LEAD_MHP_VALS]; float xyva_stds_arr[LEAD_MHP_VALS];
for (int i=0; i<LEAD_MHP_VALS; i++) { 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]); xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
} }
lead.setXyva(xyva_arr); lead.setXyva(xyva_arr);
lead.setXyvaStd(xyva_stds_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> template <class MetaBuilder>
void fill_meta(MetaBuilder meta, const float *meta_data) { void fill_meta(MetaBuilder meta, const float *meta_data) {
float desire_state_softmax[DESIRE_LEN]; 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, 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, const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
float model_execution_time) { float model_execution_time) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; 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;
MessageBuilder msg; auto framed = msg.initEvent().initModelV2();
auto framed = (msg.initEvent().*(init_model_func))(); framed.setFrameId(vipc_frame_id);
framed.setFrameId(vipc_frame_id); framed.setFrameAge(frame_age);
framed.setFrameAge(frame_age); framed.setFrameDropPerc(frame_drop * 100);
framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof);
framed.setTimestampEof(timestamp_eof); framed.setModelExecutionTime(model_execution_time);
framed.setModelExecutionTime(model_execution_time); if (send_raw_pred) {
if (send_raw_pred) { framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float)));
framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float))); }
} fill_model(framed, net_outputs);
fill_model(framed, net_outputs); pm.send("modelV2", msg);
pm.send(pub_name, msg);
};
do_publish(&cereal::Event::Builder::initModel, "model");
do_publish(&cereal::Event::Builder::initModelV2, "modelV2");
} }
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,

6
selfdrive/monitoring/dmonitoringd.py
Unescape View File

@ -12,7 +12,7 @@ def dmonitoringd_thread(sm=None, pm=None):
pm = messaging.PubMaster(['dMonitoringState']) pm = messaging.PubMaster(['dMonitoringState'])
if sm is None: 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") 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) driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill)
v_cruise_last = v_cruise v_cruise_last = v_cruise
if sm.updated['model']: if sm.updated['modelV2']:
driver_status.set_policy(sm['model']) driver_status.set_policy(sm['modelV2'])
# Get data from dmonitoringmodeld # Get data from dmonitoringmodeld
events = Events() events = Events()

28
selfdrive/test/longitudinal_maneuvers/plant.py
Unescape View File

@ -8,7 +8,7 @@ import numpy as np
from opendbc import DBC_PATH from opendbc import DBC_PATH
from cereal import car from cereal import car, log
from common.realtime import Ratekeeper from common.realtime import Ratekeeper
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
import cereal.messaging as messaging import cereal.messaging as messaging
@ -111,10 +111,10 @@ class Plant():
if not Plant.messaging_initialized: 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.logcan = messaging.pub_sock('can')
Plant.sendcan = messaging.sub_sock('sendcan') 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_params = messaging.pub_sock('liveParameters')
Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman') Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
Plant.health = messaging.pub_sock('health') Plant.health = messaging.pub_sock('health')
@ -392,13 +392,9 @@ class Plant():
# ******** publish a fake model going straight and fake calibration ******** # ******** 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 # 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: if publish_model and self.frame % 5 == 0:
md = messaging.new_message('model') md = messaging.new_message('modelV2')
cal = messaging.new_message('liveCalibration') cal = messaging.new_message('liveCalibration')
md.model.frameId = 0 md.modelV2.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
if self.lead_relevancy: if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance) d_rel = np.maximum(0., distance_lead - distance)
@ -409,15 +405,11 @@ class Plant():
v_rel = 0. v_rel = 0.
prob = 0.0 prob = 0.0
md.model.lead.dist = float(d_rel) lead = log.ModelDataV2.LeadDataV2.new_message()
md.model.lead.prob = prob lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0]
md.model.lead.relY = 0.0 lead.xyvaStd = [1.0, 1.0, 1.0, 1.0]
md.model.lead.relYStd = 1. lead.prob = prob
md.model.lead.relVel = float(v_rel) md.modelV2.leads = [lead, lead]
md.model.lead.relVelStd = 1.
md.model.lead.relA = 0.0
md.model.lead.relAStd = 10.
md.model.lead.std = 1.0
cal.liveCalibration.calStatus = 1 cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100 cal.liveCalibration.calPerc = 100

8
selfdrive/test/process_replay/camera_replay.py
Unescape View File

@ -35,7 +35,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
spinner.update("starting model replay") spinner.update("starting model replay")
pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan']) pm = messaging.PubMaster(['frame', 'liveCalibration', 'pathPlan'])
sm = messaging.SubMaster(['model', 'modelV2']) sm = messaging.SubMaster(['modelV2'])
# TODO: add dmonitoringmodeld # TODO: add dmonitoringmodeld
print("preparing procs") print("preparing procs")
@ -74,7 +74,6 @@ def camera_replay(lr, fr, desire=None, calib=None):
pm.send(msg.which(), f) pm.send(msg.which(), f)
with Timeout(seconds=15): with Timeout(seconds=15):
log_msgs.append(messaging.recv_one(sm.sock['model']))
log_msgs.append(messaging.recv_one(sm.sock['modelV2'])) log_msgs.append(messaging.recv_one(sm.sock['modelV2']))
spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count)) 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) log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit)
cmp_log = LogReader(BASE_URL + log_fn) cmp_log = LogReader(BASE_URL + log_fn)
ignore = ['logMonoTime', 'valid', 'model.frameDropPerc', 'model.modelExecutionTime', ignore = ['logMonoTime', 'valid',
'modelV2.frameDropPerc', 'modelV2.modelExecutionTime'] 'modelV2.frameDropPerc',
'modelV2.modelExecutionTime']
results: Any = {TEST_ROUTE: {}} results: Any = {TEST_ROUTE: {}}
results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, ignore_fields=ignore) results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, ignore_fields=ignore)
diff1, diff2, failed = format_diff(results, ref_commit) diff1, diff2, failed = format_diff(results, ref_commit)

6
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -222,7 +222,7 @@ CONFIGS = [
pub_sub={ pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [], "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "liveLocationKalman": [],
"model": [], "frontFrame": [], "frame": [], "ubloxRaw": [], "modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [],
}, },
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
init_callback=fingerprint, init_callback=fingerprint,
@ -233,7 +233,7 @@ CONFIGS = [
proc_name="radard", proc_name="radard",
pub_sub={ pub_sub={
"can": ["radarState", "liveTracks"], "can": ["radarState", "liveTracks"],
"liveParameters": [], "controlsState": [], "model": [], "liveParameters": [], "controlsState": [], "modelV2": [],
}, },
ignore=["logMonoTime", "valid", "radarState.cumLagMs"], ignore=["logMonoTime", "valid", "radarState.cumLagMs"],
init_callback=get_car_params, init_callback=get_car_params,
@ -266,7 +266,7 @@ CONFIGS = [
proc_name="dmonitoringd", proc_name="dmonitoringd",
pub_sub={ pub_sub={
"driverState": ["dMonitoringState"], "driverState": ["dMonitoringState"],
"liveCalibration": [], "carState": [], "model": [], "controlsState": [], "liveCalibration": [], "carState": [], "modelV2": [], "controlsState": [],
}, },
ignore=["logMonoTime", "valid"], ignore=["logMonoTime", "valid"],
init_callback=get_car_params, init_callback=get_car_params,

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
3686b117b61bfe6ad49bdef435b9112869bbf7e0 0b75e9c24662151100e77057ff7d1ffcf55d841a

3
selfdrive/test/process_replay/test_processes.py
Unescape View File

@ -158,9 +158,6 @@ if __name__ == "__main__":
if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \
(not procs_whitelisted and cfg.proc_name in args.blacklist_procs): (not procs_whitelisted and cfg.proc_name in args.blacklist_procs):
continue 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)) 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) results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs)

Write Preview
Loading…
Cancel
Save