diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 54fa41c7d0978a..248edb4c538ece 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -194,9 +194,8 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, e2e=False, desired_TR=T_FOLLOW): + def __init__(self, desired_TR=T_FOLLOW): self.dynamic_follow = DynamicFollow() - self.e2e = e2e self.desired_TR = desired_TR self.v_ego = 0. self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -232,8 +231,8 @@ def reset(self): self.x0 = np.zeros(X_DIM) self.set_weights() - def set_weights(self, prev_accel_constraint=True): - if self.e2e: + def set_weights(self, prev_accel_constraint=True, e2e=False): + if e2e: self.set_weights_for_xva_policy() self.params[:,0] = -10. self.params[:,1] = 10. @@ -269,7 +268,7 @@ def set_weights_for_lead_policy(self, prev_accel_constraint=True): self.solver.cost_set(i, 'Zl', Zl) def set_weights_for_xva_policy(self): - W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.])) + W = np.diag([0., 0., .0, 1., 0.0, 1.]) for i in range(N): self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, @@ -368,7 +367,6 @@ def update(self, carstate, radarstate, v_cruise): self.params[:,3] = np.copy(self.prev_a) self.params[:, 4] = self.desired_TR - self.run() if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and radarstate.leadOne.modelProb > 0.9): @@ -376,7 +374,7 @@ def update(self, carstate, radarstate, v_cruise): else: self.crash_cnt = 0 - def update_with_xva(self, x, v, a): + def update_with_xva(self, carstate, radarstate, v_cruise, x, v, a): # v, and a are in local frame, but x is wrt the x[0] position # In >90degree turns, x goes to 0 (and may even be -ve) # So, we use integral(v) + x[0] to obtain the forward-distance @@ -388,7 +386,32 @@ def update_with_xva(self, x, v, a): for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM]) + # set accel limits in params + self.params[:, 0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL]) + self.params[:, 1] = self.cruise_max_a + self.params[:, 2] = 1e3 self.params[:,3] = np.copy(self.prev_a) + + lead_xv_0 = self.process_lead(radarstate.leadOne) + lead_xv_1 = self.process_lead(radarstate.leadTwo) + + # To estimate a safe distance from a moving lead, we calculate how much stopping + # distance that lead needs as a minimum. We can add that to the current distance + # and then treat that as a stopped car/obstacle at this new distance. + lead_0_obstacle = lead_xv_0[:, 0] + get_stopped_equivalence_factor(lead_xv_0[:, 1]) + lead_1_obstacle = lead_xv_1[:, 0] + get_stopped_equivalence_factor(lead_xv_1[:, 1]) + + cruise_target = T_IDXS * v_cruise + x[0] + x_targets = np.column_stack([x, + lead_0_obstacle - (3 / 4) * get_safe_obstacle_distance(v), + lead_1_obstacle - (3 / 4) * get_safe_obstacle_distance(v), + cruise_target]) + + self.yref[:, 1] = np.min(x_targets, axis=1) + for i in range(N): + self.solver.set(i, "yref", self.yref[i]) + self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) + self.run() def run(self): diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8b189686b94ed4..a462a9dd8e5e53 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -7,7 +7,7 @@ from common.conversions import Conversions as CV from common.filter_simple import FirstOrderFilter from common.realtime import DT_MDL -from selfdrive.modeld.constants import T_IDXS +from selfdrive.modeld.constants import T_IDXS, IDX_N from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC @@ -48,6 +48,7 @@ class Planner: def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP self.mpc = LongitudinalMpc() + self.last_e2e = False self.fcw = False @@ -60,6 +61,10 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.solverExecutionTime = 0.0 def update(self, sm): + use_e2e_long = sm['modelLongButton'].enabled + if use_e2e_long and not self.last_e2e: + self.mpc.set_weights(e2e=True) + self.last_e2e = use_e2e_long v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise @@ -93,9 +98,26 @@ def update(self, sm): accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) self.mpc.set_weights(prev_accel_constraint) - self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) + if use_e2e_long: + self.mpc.set_accel_limits(-3.0, 2.0) + else: + self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['carState'], sm['radarState'], v_cruise) + + if use_e2e_long: + if (len(sm['modelV2'].position.x) == IDX_N and + len(sm['modelV2'].velocity.x) == IDX_N and + len(sm['modelV2'].acceleration.x) == IDX_N): + x = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].position.x) + v = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].velocity.x) + a = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].acceleration.x) + else: + x = np.zeros(len(T_IDXS_MPC)) + v = np.zeros(len(T_IDXS_MPC)) + a = np.zeros(len(T_IDXS_MPC)) + self.mpc.update_with_xva(sm['carState'], sm['radarState'], v_cruise, x, v, a) + else: + self.mpc.update(sm['carState'], sm['radarState'], v_cruise) self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) @@ -125,7 +147,8 @@ def publish(self, sm, pm): longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source + if not sm['modelLongButton'].enabled: + longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw longitudinalPlan.solverExecutionTime = self.mpc.solve_time diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 59c0b249d9d32a..64156bf52c73d8 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -199,6 +199,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic std::array pos_x, pos_y, pos_z; std::array pos_x_std, pos_y_std, pos_z_std; std::array vel_x, vel_y, vel_z; + std::array accel_x, accel_y, accel_z; std::array rot_x, rot_y, rot_z; std::array rot_rate_x, rot_rate_y, rot_rate_z; @@ -212,6 +213,9 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic vel_x[i] = plan.mean[i].velocity.x; vel_y[i] = plan.mean[i].velocity.y; vel_z[i] = plan.mean[i].velocity.z; + accel_x[i] = plan.mean[i].acceleration.x; + accel_y[i] = plan.mean[i].acceleration.y; + accel_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; @@ -222,6 +226,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic 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, accel_x, accel_y, accel_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); } diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 13025a9f03b38d..c787fca437265b 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -53,6 +53,7 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): radar = messaging.new_message('radarState') control = messaging.new_message('controlsState') car_state = messaging.new_message('carState') + model_long_button = messaging.new_message('modelLongButton') a_lead = (v_lead - self.v_lead_prev)/self.ts self.v_lead_prev = v_lead @@ -94,7 +95,8 @@ def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): # ******** get controlsState messages for plotting *** sm = {'radarState': radar.radarState, 'carState': car_state.carState, - 'controlsState': control.controlsState} + 'controlsState': control.controlsState, + 'modelLongButton': model_long_button.modelLongButton} self.planner.update(sm) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index f6e01c11295087..4bb338c96c956b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -122,7 +122,7 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) { main_layout->addWidget(btns_wrapper, 0, Qt::AlignBottom); // Model long button - mlButton = new QPushButton("Model Cruise Control"); + mlButton = new QPushButton("Toggle Model Long"); QObject::connect(mlButton, &QPushButton::clicked, [=]() { uiState()->scene.mlButtonEnabled = !mlEnabled; }); @@ -132,7 +132,7 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) { btns_layout->addWidget(mlButton, 0, Qt::AlignHCenter | Qt::AlignBottom); btns_layout->addStretch(3); - std::string hide_model_long = "true"; // util::read_file("/data/community/params/hide_model_long"); + std::string hide_model_long = util::read_file("/data/community/params/hide_model_long"); if (hide_model_long == "true"){ mlButton->hide(); }