From b7b87efed99a2fc784a579e7baa59e56cbfc963d Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Thu, 12 Aug 2021 21:06:26 -0700 Subject: [PATCH] Retuned desire model (#21919) * New model: d8e7f76f-7bec-4a83-af00-c0fae792527f/950 * Updated process replay refs * Updated model replay ref old-commit-hash: 05b37552f3a38f914af41f44ccc7c633ad152a15 --- models/supercombo.dlc | 4 +- models/supercombo.onnx | 4 +- selfdrive/common/modeldata.h | 2 +- selfdrive/controls/lib/lateral_planner.py | 11 +++++ selfdrive/controls/lib/radar_helpers.py | 10 ++-- selfdrive/controls/radard.py | 16 +++---- selfdrive/modeld/models/driving.cc | 46 ++++++++++++++----- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/ui/paint.cc | 12 ++--- selfdrive/ui/ui.cc | 11 ++--- 11 files changed, 76 insertions(+), 44 deletions(-) diff --git a/models/supercombo.dlc b/models/supercombo.dlc index 676a2ace37..29c282c03e 100644 --- a/models/supercombo.dlc +++ b/models/supercombo.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:dc46a24d4b4afa9730785264834e7f7c04c84b6a28a689acb503d6663818c256 -size 58797567 +oid sha256:1c53859f4d15a172811e0af815f192c272072005366c1cb9d05b819f19a6c48d +size 56720671 diff --git a/models/supercombo.onnx b/models/supercombo.onnx index 6b3ec952c1..6d2a81b5b1 100644 --- a/models/supercombo.onnx +++ b/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:77a31e5a3a70c39a3fcb07e939a668baf80b1eb778fe792fbe256de983fab5cd -size 58823636 +oid sha256:69c1f8f71fd815c9d30361b99b1dfd39df5460176c628c038d3f7d91e4801704 +size 56742706 diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 4eb3068054..49e200d543 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -1,7 +1,7 @@ #pragma once const int TRAJECTORY_SIZE = 33; -const int LON_MPC_N = 32; const int LAT_MPC_N = 16; +const int LON_MPC_N = 32; const float MIN_DRAW_DISTANCE = 10.0; const float MAX_DRAW_DISTANCE = 100.0; diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 0c258d8dba..1c3ca89898 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -55,6 +55,7 @@ class LateralPlanner(): self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 + self.keep_pulse_timer = 0.0 self.prev_one_blinker = False self.desire = log.LateralPlan.Desire.none @@ -157,6 +158,16 @@ class LateralPlanner(): self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + # Send keep pulse once per second during LaneChangeStart.preLaneChange + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.laneChangeStarting]: + self.keep_pulse_timer = 0.0 + elif self.lane_change_state == LaneChangeState.preLaneChange: + self.keep_pulse_timer += DT_MDL + if self.keep_pulse_timer > 1.0: + self.keep_pulse_timer = 0.0 + elif self.desire in [log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight]: + self.desire = log.LateralPlan.Desire.none + # Turn off lanes during lane change if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: self.LP.lll_prob *= self.lane_change_ll_prob diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 132bba8bd3..01498fa138 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -132,11 +132,11 @@ class Cluster(): def get_RadarState_from_vision(self, lead_msg, v_ego): return { - "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]), + "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.y[0]), + "vRel": float(lead_msg.v[0] - v_ego), + "vLead": float(lead_msg.v[0]), + "vLeadK": float(lead_msg.v[0]), "aLeadK": float(0), "aLeadTau": _LEAD_ACCEL_TAU, "fcw": False, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 244d02d23f..5d94804d75 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -38,12 +38,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.xyva[0] - RADAR_TO_CAMERA + offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): - 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]) + prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0]) + prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0]) + prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v @@ -53,7 +53,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.xyva[2]) < 10) or (v_ego + cluster.vRel > 3) + vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) if dist_sane and vel_sane: return cluster else: @@ -166,9 +166,9 @@ class RadarD(): radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: - 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) + if len(sm['modelV2'].leadsV3) > 1: + radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[0], low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[1], low_speed_override=False) return dat diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index f7a72b34e1..3fb6f56255 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -24,7 +24,9 @@ constexpr int PLAN_MHP_SELECTION = 1; constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION); constexpr int LEAD_MHP_N = 5; -constexpr int LEAD_MHP_VALS = 4; +constexpr int LEAD_TRAJ_LEN = 6; +constexpr int LEAD_PRED_DIM = 4; +constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN; constexpr int LEAD_MHP_SELECTION = 3; constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION); @@ -147,18 +149,38 @@ void fill_sigmoid(const float *input, float *output, int len, int stride) { } } -void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) { +void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) { + float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; const float *data = get_lead_data(lead_data, t_offset); lead.setProb(sigmoid(prob[t_offset])); - lead.setT(t); - float xyva_arr[LEAD_MHP_VALS]; - float xyva_stds_arr[LEAD_MHP_VALS]; - for (int i=0; iscene.longitudinal_control) { - auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeads()[0]; - auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeads()[1]; + auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0]; + auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1]; if (lead_one.getProb() > .5) { draw_lead(s, lead_one, s->scene.lead_vertices[0]); } - if (lead_two.getProb() > .5 && (std::abs(lead_one.getXyva()[0] - lead_two.getXyva()[0]) > 3.0)) { + if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) { draw_lead(s, lead_two, s->scene.lead_vertices[1]); } } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 8187d335a8..1520aea3c2 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -64,13 +64,12 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line } static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) { - auto leads = model.getLeads(); + auto leads = model.getLeadsV3(); auto model_position = model.getPosition(); for (int i = 0; i < 2; ++i) { if (leads[i].getProb() > 0.5) { - auto xyva = leads[i].getXyva(); - float z = model_position.getZ()[get_path_length_idx(model_position, xyva[0])]; - calib_frame_to_full_frame(s, xyva[0], xyva[1], z + 1.22, &s->scene.lead_vertices[i]); + float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])]; + calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[i]); } } } @@ -113,9 +112,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { } // update path - auto lead_one = model.getLeads()[0]; + auto lead_one = model.getLeadsV3()[0]; if (lead_one.getProb() > 0.5) { - const float lead_d = lead_one.getXyva()[0] * 2.; + const float lead_d = lead_one.getX()[0] * 2.; max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance);