diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 34fc85f8a55c6c..c7190405866f08 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,6 +2,7 @@ import math import numpy as np +from cereal import log import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.constants import CV @@ -9,30 +10,120 @@ from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc -from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET -from openpilot.common.swaglog import cloudlog +#from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU +from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value -LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] -CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] +A_CRUISE_MIN_VALS = [-1.2,] +A_CRUISE_MIN_BP = [0., ] +ACC_ACCEL_MAX = 2.0 +ACCEL_MIN = -3.5 + ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 +#FCW_IDXS = ModelConstants.T_IDXS < 5.0 +#T_DIFFS = np.diff(T_IDXS, prepend=[0.]) +COMFORT_BRAKE = 2.5 +STOP_DISTANCE = 6.0 + + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +T_IDXS = np.array(ModelConstants.T_IDXS) +FCW_IDXS = T_IDXS < 5.0 +T_IDXS_LEAD = np.arange(0., 2.5, 0.1) +T_DIFFS_LEAD = np.diff(T_IDXS_LEAD, prepend=[0.]) + + +def get_jerk_factor(personality=log.LongitudinalPersonality.standard): + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") + + +def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") + +def get_stopped_equivalence_factor(v_lead): + return (v_lead**2) / (2 * COMFORT_BRAKE) + +def get_safe_obstacle_distance(v_ego, t_follow): + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + +def get_desired_follow_distance(v_ego, v_lead, t_follow=None): + if t_follow is None: + t_follow = get_T_FOLLOW() + return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) + + def get_max_accel(v_ego): return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) 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 extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): + a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS_LEAD**2)/2.) + v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS_LEAD * a_lead_traj), 0.0, 1e8) + x_lead_traj = x_lead + np.cumsum(T_DIFFS_LEAD * v_lead_traj) + lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) + return lead_xv + +def process_lead(v_ego, lead): + if lead is not None and lead.status: + x_lead = lead.dRel + v_lead = lead.vLead + a_lead = lead.aLeadK + a_lead_tau = lead.aLeadTau + else: + # Fake a fast lead car, so mpc can keep running in the same mode + x_lead = 50.0 + v_lead = v_ego + 10.0 + a_lead = 0.0 + a_lead_tau = _LEAD_ACCEL_TAU + + # MPC will not converge if immediate crash is expected + # Clip lead distance to what is still possible to brake for + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + x_lead = np.clip(x_lead, min_x_lead, 1e8) + v_lead = np.clip(v_lead, 0.0, 1e8) + a_lead = np.clip(a_lead, -10., 5.) + lead_xv = extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) + return lead_xv + +def process_ego(v_ego, a_ego): + x_lead = 0.0 + v_lead = v_ego + a_lead = a_ego + a_lead_tau = _LEAD_ACCEL_TAU + + # MPC will not converge if immediate crash is expected + # Clip lead distance to what is still possible to brake for + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + x_lead = np.clip(x_lead, min_x_lead, 1e8) + v_lead = np.clip(v_lead, 0.0, 1e8) + a_lead = np.clip(a_lead, -10., 5.) + lead_xv = extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) + return lead_xv def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ @@ -51,43 +142,25 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc(dt=dt) # TODO remove mpc modes when TR released - self.mpc.mode = 'acc' self.fcw = False self.dt = dt self.allow_throttle = True 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.output_a_target = 0.0 self.output_should_stop = False - self.v_desired_trajectory = np.zeros(CONTROL_N) - self.a_desired_trajectory = np.zeros(CONTROL_N) - self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 @staticmethod def parse_model(model_msg): - if (len(model_msg.position.x) == ModelConstants.IDX_N and - len(model_msg.velocity.x) == ModelConstants.IDX_N and - len(model_msg.acceleration.x) == ModelConstants.IDX_N): - x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) - j = np.zeros(len(T_IDXS_MPC)) - else: - x = np.zeros(len(T_IDXS_MPC)) - v = np.zeros(len(T_IDXS_MPC)) - a = np.zeros(len(T_IDXS_MPC)) - j = np.zeros(len(T_IDXS_MPC)) if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1: throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1] else: throttle_prob = 1.0 - return x, v, a, j, throttle_prob + return throttle_prob def update(self, sm): mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' @@ -111,7 +184,7 @@ def update(self, sm): reset_state = reset_state or not v_cruise_initialized # No change cost when user is controlling the speed, or when standstill - prev_accel_constraint = not (reset_state or sm['carState'].standstill) + #prev_accel_constraint = not (reset_state or sm['carState'].standstill) if mode == 'acc': accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] @@ -123,11 +196,11 @@ def update(self, sm): 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]) + self.output_a_target = 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)) - x, v, a, j, throttle_prob = self.parse_model(sm['modelV2']) + throttle_prob = self.parse_model(sm['modelV2']) # 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 @@ -139,41 +212,56 @@ def update(self, sm): 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, x, v, a, j, 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) - self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone - self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill - if self.fcw: - cloudlog.info("FCW triggered") + #self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill + #if self.fcw: + # cloudlog.info("FCW triggered") # Interpolate 0.05 seconds and save as 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 + #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, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) - output_a_target_e2e = sm['modelV2'].action.desiredAcceleration - output_should_stop_e2e = sm['modelV2'].action.shouldStop + #action_t = self.CP.longitudinalActuatorDelay + DT_MDL + out_accels = {} + cruise_min = np.interp(v_ego, A_CRUISE_MIN_BP, A_CRUISE_MIN_VALS) + cruise_max = np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) + cruise_accel = 0.5*(v_cruise - v_ego) + cruise_accel = np.clip(cruise_accel, cruise_min, cruise_max) + cruise_accel = smooth_value(cruise_accel, self.output_a_target, 1.0) + out_accels['cruise'] = (cruise_accel, False) - if mode == 'acc': - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc - else: - output_a_target = min(output_a_target_mpc, output_a_target_e2e) - self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc - 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 + + lead_info = {'lead0': sm['radarState'].leadOne, 'lead1': sm['radarState'].leadTwo} + ego_xv = process_ego(v_ego, sm['carState'].aEgo) + for key in lead_info.keys(): + lead_xv = process_lead(v_ego, lead_info[key]) + + + ## 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. + desired_follow_distance = np.array([get_desired_follow_distance(v_ego, lead_xv[0,1], get_T_FOLLOW(sm['selfdriveState'].personality)) for v, vl in zip(ego_xv, lead_xv)]) + + error_weights = np.linspace(1.0, 0.0, len(desired_follow_distance)) + error_weights = error_weights / np.sum(error_weights) + + follow_distance_error = np.sum(error_weights*(lead_xv[:,0] - desired_follow_distance)) + + follow_distance_cost_signed = (follow_distance_error / (v_ego + 1))**2 * np.sign(follow_distance_error) + lead_accel = np.clip(2*follow_distance_cost_signed, ACCEL_MIN, ACCEL_MAX) + lead_accel = smooth_value(lead_accel, self.output_a_target, 0.5) + + out_accels[key] = (lead_accel, False) + + if mode == 'blended': + out_accels['model']= (sm['modelV2'].action.desiredAcceleration, sm['modelV2'].action.shouldStop) + + output_a_target = np.min([x for x, _ in out_accels.values()]) + self.output_should_stop = np.all([x for _, x in out_accels.values()]) + self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1]) def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') @@ -182,15 +270,9 @@ def publish(self, sm, pm): longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] - longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] - longitudinalPlan.solverExecutionTime = self.mpc.solve_time - - longitudinalPlan.speeds = self.v_desired_trajectory.tolist() - longitudinalPlan.accels = self.a_desired_trajectory.tolist() - longitudinalPlan.jerks = self.j_desired_trajectory.tolist() longitudinalPlan.hasLead = sm['radarState'].leadOne.status - longitudinalPlan.longitudinalPlanSource = self.mpc.source + #longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw longitudinalPlan.aTarget = float(self.output_a_target) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index 33b363cfd9496c..e9fc2900ee2f3f 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -13,7 +13,6 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.modeld.fill_model_msg import fill_xyz_poly, fill_lane_line_meta from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_from_plan, CONTROL_N_T_IDX from openpilot.system.manager.process_config import managed_processes from openpilot.tools.lib.logreader import LogIterable @@ -109,7 +108,7 @@ def migrate_longitudinalPlan(msgs): if msg.which() != 'longitudinalPlan': continue new_msg = msg.as_builder() - a_target, should_stop = get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) + a_target, should_stop = 0.0, False #get_accel_from_plan(msg.longitudinalPlan.speeds, msg.longitudinalPlan.accels, CONTROL_N_T_IDX) new_msg.longitudinalPlan.aTarget, new_msg.longitudinalPlan.shouldStop = float(a_target), bool(should_stop) ops.append((index, new_msg.as_reader())) return ops, [], []