diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index de1ba0c278..725491eed2 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -23,7 +23,7 @@ EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource -MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise) +MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) X_DIM = 3 U_DIM = 1 @@ -55,8 +55,6 @@ FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -CRUISE_MIN_ACCEL = -1.2 -CRUISE_MAX_ACCEL = 1.6 MIN_X_LEAD_FACTOR = 0.5 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): @@ -309,9 +307,8 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) - v_ego = self.x0[1] lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) @@ -322,15 +319,7 @@ class LongitudinalMpc: 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]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) - # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] self.yref[:,:] = 0.0 diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 116de14e0e..0b1fee329d 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -17,6 +17,8 @@ from openpilot.common.swaglog import cloudlog A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] +J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6] +A_CRUISE_MIN = -1.2 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 @@ -31,18 +33,26 @@ def get_max_accel(v_ego): def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py -def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): - """ - This function returns a limited long acceleration allowed, depending on the existing lateral acceleration - this should avoid accelerating when losing the target in turns - """ - # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel - # The lookup table for turns should also be updated if we do this - a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) - a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) +def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle): + max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) - return [a_target[0], min(a_target[1], a_x_allowed)] + if not e2e: + a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) + a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) + a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) + max_accel = min(max_accel, a_x_allowed) + if not allow_throttle: + clipped_accel_coast = max(accel_coast, ACCEL_MIN) + coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast]) + max_accel = min(max_accel, coast_limit) + + target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel) + if not e2e: + j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS) + target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt)) + + cruise_should_stop = v_cruise == 0.0 + return target_accel, cruise_should_stop class LongitudinalPlanner: @@ -55,7 +65,7 @@ class LongitudinalPlanner: self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) - self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] + self.a_cruise = 0.0 self.output_a_target = 0.0 self.output_should_stop = False @@ -72,46 +82,36 @@ class LongitudinalPlanner: v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS - v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET + if sm['controlsState'].forceDecel: + v_cruise = 0.0 long_control_off = sm['controlsState'].longControlState == LongCtrlState.off - force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled # PCM cruise speed may be updated a few cycles later, check if initialized + v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET reset_state = reset_state or not v_cruise_initialized + throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs + throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0 + self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED + + steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg + + if reset_state: + self.v_desired_filter.x = v_ego + self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX) + + # Prevent divergence, smooth in current v_ego + self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) + # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] - steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg - accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) - - if reset_state: - self.v_desired_filter.x = v_ego - # Clip aEgo to cruise limits to prevent large accelerations when becoming active - self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1]) - - # Prevent divergence, smooth in current v_ego - self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs - throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0 - # Don't clip at low speeds since throttle_prob doesn't account for creep - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED - - if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_clip[0]) - clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast]) - accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp) - - if force_slow_decel: - v_cruise = 0.0 - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -122,10 +122,8 @@ class LongitudinalPlanner: if self.fcw: cloudlog.info("FCW triggered") - # Interpolate 0.05 seconds and save as starting point for next iteration + # Save starting point for next iteration a_prev = self.a_desired - self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) - self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 action_t = self.CP.longitudinalActuatorDelay + DT_MDL output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, @@ -133,19 +131,21 @@ class LongitudinalPlanner: output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if sm['selfdriveState'].experimentalMode: - output_a_target = min(output_a_target_e2e, output_a_target_mpc) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - if output_a_target < output_a_target_mpc: - self.mpc.source = LongitudinalPlanSource.e2e - else: - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc + self.a_cruise, cruise_should_stop = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego, + self.a_cruise, steer_angle_without_offset, self.CP, self.dt, + accel_coast, self.allow_throttle) - for idx in range(2): - accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05) - self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) - self.prev_accel_clip = accel_clip + candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), + (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] + if sm['selfdriveState'].experimentalMode: + candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) + + output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) + self.output_should_stop = any(should_stop for _, _, should_stop in candidates) + self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX) + + self.a_desired = float(self.output_a_target) + self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/maneuver.py b/openpilot/selfdrive/test/longitudinal_maneuvers/maneuver.py index ba0379f2d7..99a7d8d690 100644 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/maneuver.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -43,6 +43,7 @@ class Maneuver: valid = True logs = [] + not_starting_t = 0.0 while plant.current_time < self.duration: speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) prob_lead = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) @@ -68,8 +69,13 @@ class Maneuver: valid = False if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3: - print('LongitudinalPlanner not starting!') - valid = False + if not_starting_t == 0.0: + not_starting_t = plant.current_time + elif plant.current_time - not_starting_t > 0.5: + print('LongitudinalPlanner not starting!') + valid = False + else: + not_starting_t = 0.0 if self.ensure_slowdown and log['speed'] > 5.5: print('LongitudinalPlanner not slowing down!') diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py index 23ccbdb85d..bdd5c51ee4 100755 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -107,7 +107,7 @@ class Plant: position = log.XYZTData.new_message() position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] model.modelV2.position = position - model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1) + model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.5) velocity = log.XYZTData.new_message() velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] velocity.x[0] = float(self.speed) # always start at current speed diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/openpilot/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 9bd22902d3..c8694b6ac2 100644 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -151,7 +151,9 @@ def create_maneuvers(kwargs): enabled=False, **kwargs, ), - Maneuver( + ] + if not kwargs['e2e']: + maneuvers.append(Maneuver( "slow to 5m/s with allow_throttle = False and pitch = +0.1", duration=30., initial_speed=20., @@ -162,7 +164,7 @@ def create_maneuvers(kwargs): breakpoints=[0.0, 2., 2.01], ensure_slowdown=True, **kwargs, - )] + )) if not kwargs['force_decel']: # controls relies on planner commanding to move for stock-ACC resume spamming maneuvers.append(Maneuver(