Acodonomo minimal

selfdrive/controls/lib/longitudinal_planner.py

@@ -2,37 +2,128 @@
 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
 from openpilot.common.filter_simple import FirstOrderFilter
 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)

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, [], []