From 677c0208030ac8099ab4a0dffda375dfa01cb018 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 12 Jan 2021 15:08:56 -0700 Subject: [PATCH] 08 e2e (#94) * e2e Co-Authored-By: Shane Smiskol * enable alpha e2e long with 0.8. doesn't work very well yet * add modelv2 Co-Authored-By: Shane Smiskol * spelling * forgot * fix * dfix * syntax * fix * how about this * gix * fix * fix * how about this. * fix * hmm * revert * fix Co-authored-by: Shane Smiskol --- cereal/log.capnp | 5 ++++ cereal/service_list.yaml | 2 ++ selfdrive/controls/lib/planner.py | 46 ++++++++++++++++++++++++++++--- selfdrive/controls/plannerd.py | 2 +- selfdrive/ui/android/ui.cc | 35 +++++++++++++++++++++++ selfdrive/ui/paint.cc | 29 +++++++++++++++++++ selfdrive/ui/ui.cc | 30 ++++++++++++++++++++ selfdrive/ui/ui.hpp | 5 +++- 8 files changed, 148 insertions(+), 6 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index bc2d5de6f236bb..c719a83e8649cf 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2066,6 +2066,10 @@ struct Sentinel { type @0 :SentinelType; } +struct ModelLongButton { + enabled @0 :Bool; +} + struct Event { # in nanoseconds? logMonoTime @0 :UInt64; @@ -2150,6 +2154,7 @@ struct Event { wideEncodeIdx @77 :EncodeIndex; dragonConf @78 :DragonConf; liveTrafficData @79:LiveTrafficData; + modelLongButton @80 :ModelLongButton; } } diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml index c0af1611f48d1a..97046ae5da9191 100644 --- a/cereal/service_list.yaml +++ b/cereal/service_list.yaml @@ -83,6 +83,8 @@ modelV2: [8077, true, 20., 20] dragonConf: [8088, false, 2.] +modelLongButton: [8084, false, 0.] + testModel: [8040, false, 0.] testLiveLocation: [8045, false, 0.] testJoystick: [8056, false, 0.] diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index fee2399e8b2875..c1efc80dd6035c 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -17,6 +17,7 @@ from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX +from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel from common.op_params import opParams op_params = opParams() osm = op_params.get('osm') @@ -117,6 +118,7 @@ def __init__(self, CP): self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) + self.mpc_model = LongitudinalMpcModel() self.v_acc_start = 0.0 self.a_acc_start = 0.0 @@ -126,6 +128,8 @@ def __init__(self, CP): self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 + self.v_model = 0.0 + self.a_model = 0.0 self.osm = True self.longitudinalPlanSource = 'cruise' @@ -144,7 +148,8 @@ def __init__(self, CP): self.v_model = 0.0 self.a_model = 0.0 - def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle): + def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAngle, model_enabled): + #possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint] center_x = -2.5 # Wheel base 2.5m lead1_check = True lead2_check = True @@ -166,6 +171,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status and lead2_check: solutions['mpc2'] = self.mpc2.v_mpc + if self.mpc_model.valid and model_enabled: + solutions['model'] = self.mpc_model.v_mpc + solutions['cruise'] = self.v_cruise slowest = min(solutions, key=solutions.get) @@ -180,6 +188,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise + elif slowest == 'model': + self.v_acc = self.mpc_model.v_mpc + self.a_acc = self.mpc_model.a_mpc # dp - slow on curve from 0.7.6.1 elif self.dp_slow_on_curve and slowest == 'model': self.v_acc = self.v_model @@ -187,9 +198,29 @@ def choose_solution(self, v_cruise_setpoint, enabled, lead_1, lead_2, steeringAn self.v_acc_future = v_cruise_setpoint if lead1_check: - self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future]) + self.v_acc_future = min([self.mpc1.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future]) if lead2_check: - self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future]) + self.v_acc_future = min([self.mpc2.v_mpc_future, self.v_acc_future, self.mpc_model.v_mpc_future ]) + + def parse_modelV2_data(self, sm): + modelV2 = sm['modelV2'] + distances, speeds, accelerations = [], [], [] + if not sm.updated['modelV2'] or len(modelV2.position.x) == 0: + return distances, speeds, accelerations + + model_t = modelV2.position.t + mpc_times = list(range(10)) + + model_t_idx = [sorted(range(len(model_t)), key=[abs(idx - t) for t in model_t].__getitem__)[0] for idx in mpc_times] # matches 0 to 9 interval to idx from t + + for t in model_t_idx: # everything is derived from x position since velocity is outputting weird values + speeds.append(modelV2.velocity.x[t]) + distances.append(modelV2.position.x[t]) + if model_t_idx.index(t) > 0: # skip first since we can't calculate (and don't want to use v_ego) + accelerations.append((speeds[-1] - speeds[-2]) / model_t[t]) + + accelerations.insert(0, accelerations[1] - (accelerations[2] - accelerations[1])) # extrapolate back first accel from second and third, less weight + return distances, speeds, accelerations def update(self, sm, pm, CP, VM, PP): """Gets called when new radarState is available""" @@ -360,11 +391,18 @@ def update(self, sm, pm, CP, VM, PP): self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start) + self.mpc_model.set_cur_state(self.v_acc_start, self.a_acc_start) self.mpc1.update(pm, sm['carState'], lead_1) self.mpc2.update(pm, sm['carState'], lead_2) - self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle) + distances, speeds, accelerations = self.parse_modelV2_data(sm) + self.mpc_model.update(sm['carState'].vEgo, sm['carState'].aEgo, + distances, + speeds, + accelerations) + + self.choose_solution(v_cruise_setpoint, enabled, lead_1, lead_2, sm['carState'].steeringAngle, sm['modelLongButton'].enabled) # determine fcw if self.mpc1.new_lead: diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 2f1e9467622f9f..53d698d6abd032 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -23,7 +23,7 @@ def plannerd_thread(sm=None, pm=None): VM = VehicleModel(CP) if sm is None: - sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData'], + sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'model', 'liveParameters', 'dragonConf', 'liveMapData', 'modelLongButton', 'modelV2'], poll=['radarState', 'model']) if pm is None: diff --git a/selfdrive/ui/android/ui.cc b/selfdrive/ui/android/ui.cc index 95b72719697ce4..d6f19a651ad8d6 100644 --- a/selfdrive/ui/android/ui.cc +++ b/selfdrive/ui/android/ui.cc @@ -2,6 +2,7 @@ #include #include #include +#include #include @@ -29,6 +30,29 @@ static void ui_set_brightness(UIState *s, int brightness) { } } +static void send_ml(UIState *s, bool enabled) { + MessageBuilder msg; + auto mlStatus = msg.initEvent().initModelLongButton(); + mlStatus.setEnabled(enabled); + s->pm->send("modelLongButton", msg); +} + +static bool handle_ml_touch(UIState *s, int touch_x, int touch_y) { + //mlButton manager + int padding = 40; + int btn_w = 500; + int btn_h = 138; + int xs[2] = {1920 / 2 - btn_w / 2, 1920 / 2 + btn_w / 2}; + int y_top = 915 - btn_h / 2; + if (xs[0] <= touch_x + padding && touch_x - padding <= xs[1] && y_top - padding <= touch_y) { + s->scene.mlButtonEnabled = !s->scene.mlButtonEnabled; + send_ml(s, s->scene.mlButtonEnabled); + printf("ml button: %d\n", s->scene.mlButtonEnabled); + return true; + } + return false; +} + static void handle_display_state(UIState *s, bool user_input) { static int awake_timeout = 0; @@ -103,6 +127,10 @@ static bool handle_dp_btn_touch(UIState *s, int touch_x, int touch_y) { Params().write_db_value("dp_last_modified", time_str, 11); return true; } + if (handle_ml_touch(s, touch_x, touch_y)) { + s->scene.uilayout_sidebarcollapsed = true; // collapse sidebar when tapping any SA button + return true; // only allow one button to be pressed at a time + } } return false; } @@ -161,6 +189,7 @@ int main(int argc, char* argv[]) { UIState uistate = {}; UIState *s = &uistate; ui_init(s); + sa_init(s, true); s->sound = &sound; TouchState touch = {0}; @@ -196,6 +225,12 @@ int main(int argc, char* argv[]) { if (!s->started) { usleep(50 * 1000); } + + if (s->started) { + sa_init(s, false); // reset ml button and regrab params + } + + double u1 = millis_since_boot(); ui_update(s); diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index c0ee6f989f75cc..d7ea19ab33ad79 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -548,6 +548,34 @@ static void ui_draw_driver_view(UIState *s) { ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected()); } +static void ui_draw_ml_button(UIState *s) { + int btn_w = 500; + int btn_h = 138; + int x = 1920 / 2; + int y = 915; + int btn_x = x - btn_w / 2; + int btn_y = y - btn_h / 2; + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25); + if (s->scene.mlButtonEnabled) { // change outline color based on status of button + nvgStrokeColor(s->vg, nvgRGBA(55, 184, 104, 255)); + } else { + nvgStrokeColor(s->vg, nvgRGBA(184, 55, 55, 255)); + } + nvgStrokeWidth(s->vg, 12); + nvgStroke(s->vg); + + nvgBeginPath(s->vg); // dark background for readability + nvgRoundedRect(s->vg, btn_x, btn_y, btn_w, btn_h, 25); + nvgFillColor(s->vg, nvgRGBA(75, 75, 75, 75)); + nvgFill(s->vg); + + nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255)); + nvgFontSize(s->vg, 65); + nvgText(s->vg, x, y + btn_h / 8, "Toggle Model Long", NULL); +} + static void ui_draw_vision_header(UIState *s) { const Rect &viz_rect = s->scene.viz_rect; if (!s->scene.dpFullScreenApp) { @@ -567,6 +595,7 @@ static void ui_draw_vision_header(UIState *s) { if (s->scene.dpUiEvent) { ui_draw_vision_event(s); } + ui_draw_ml_button(s); } static void ui_draw_vision_footer(UIState *s) { diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 0b2a110bacd8ad..8eb966df22c1b1 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -8,6 +8,8 @@ #include #include #include +//#include "json11.hpp" +//#include #include "common/util.h" #include "common/swaglog.h" @@ -24,6 +26,34 @@ int write_param_float(float param, const char* param_name, bool persistent_param return Params(persistent_param).write_db_value(param_name, s, size < sizeof(s) ? size : sizeof(s)); } +void sa_init(UIState *s, bool full_init) { + if (full_init) { + s->pm = new PubMaster({"modelLongButton"}); + } + + //s->ui_debug = false; // change to true while debugging + + // stock additions todo: run opparams first (in main()?) to ensure json values exist + //std::ifstream op_params_file("/data/op_params.json"); + //std::string op_params_content((std::istreambuf_iterator(op_params_file)), + //(std::istreambuf_iterator())); + + //std::string err; + //auto json = json11::Json::parse(op_params_content, err); + //if (!json.is_null() && err.empty()) { + //printf("successfully parsed opParams json\n"); + //s->scene.dfButtonStatus = DF_TO_IDX[json["dynamic_follow"].string_value()]; + //s->scene.lsButtonStatus = LS_TO_IDX[json["lane_speed_alerts"].string_value()]; +// printf("dfButtonStatus: %d\n", s->scene.dfButtonStatus); +// printf("lsButtonStatus: %d\n", s->scene.lsButtonStatus); + //} else { // error parsing json + //printf("ERROR PARSING OPPARAMS JSON!\n"); + //s->scene.dfButtonStatus = 0; + //s->scene.lsButtonStatus = 0; + //} + s->scene.mlButtonEnabled = false; // state isn't saved yet +} + void ui_init(UIState *s) { s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "liveMapData", "health", "carParams", "ubloxGnss", "driverState", "dMonitoringState", "sensorEvents", diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 18a743559861ac..39477ea7eadc93 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -107,6 +107,8 @@ typedef struct { typedef struct UIScene { + bool mlButtonEnabled; + mat4 extrinsic_matrix; // Last row is 0 so we can use mat4. bool world_objects_visible; @@ -232,7 +234,7 @@ typedef struct UIState { int img_speed; SubMaster *sm; - + PubMaster *pm; Sound *sound; UIStatus status; UIScene scene; @@ -278,6 +280,7 @@ typedef struct UIState { } UIState; void ui_init(UIState *s); +void sa_init(UIState *s, bool full_init); void ui_update(UIState *s); int write_param_float(float param, const char* param_name, bool persistent_param = false);