FrogPilot setup - FrogPilot Planner

2026-08-05 00:05:59 +08:00 · 2024-07-23 23:10:07 -07:00
parent 27f9288a0f
commit ec3d96f1de
4 changed files with 123 additions and 14 deletions
@@ -59,11 +59,11 @@ STOP_DISTANCE = 6.0

 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
  if personality==log.LongitudinalPersonality.relaxed:
-    return 1.0
+    return 1.0, 1.0, 1.0
  elif personality==log.LongitudinalPersonality.standard:
-    return 1.0
+    return 1.0, 1.0, 1.0
  elif personality==log.LongitudinalPersonality.aggressive:
-    return 0.5
+    return 0.5, 1.0, 0.5
  else:
    raise NotImplementedError("Longitudinal personality not supported")

@@ -273,12 +273,11 @@ class LongitudinalMpc:
    for i in range(N):
      self.solver.cost_set(i, 'Zl', Zl)

-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
-    jerk_factor = get_jerk_factor(personality)
+  def set_weights(self, acceleration_jerk=1.0, danger_jerk = 1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
    if self.mode == 'acc':
-      a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
-      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+      a_change_cost = acceleration_jerk if prev_accel_constraint else 0
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk]
    elif self.mode == 'blended':
      a_change_cost = 40.0 if prev_accel_constraint else 0
      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -332,8 +331,7 @@ class LongitudinalMpc:
    self.cruise_min_a = min_a
    self.max_a = max_a

-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-    t_follow = get_T_FOLLOW(personality)
+  def update(self, radarstate, v_cruise, x, v, a, j, t_follow, personality=log.LongitudinalPersonality.standard):
    v_ego = self.x0[1]
    self.status = radarstate.leadOne.status or radarstate.leadTwo.status

@@ -111,11 +111,10 @@ class LongitudinalPlanner:
    # No change cost when user is controlling the speed, or when standstill
    prev_accel_constraint = not (reset_state or sm['carState'].standstill)

+    accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
    if self.mpc.mode == 'acc':
-      accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
      accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
    else:
-      accel_limits = [ACCEL_MIN, ACCEL_MAX]
      accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]

    if reset_state:
@@ -134,11 +133,12 @@ class LongitudinalPlanner:
    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)

-    self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality)
+    self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality)
    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)
    x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
+    self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
+                    personality=sm['controlsState'].personality)

    self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -0,0 +1,109 @@
+import cereal.messaging as messaging
+
+from cereal import car
+
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_UNSET
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, COMFORT_BRAKE, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE, \
+                                                                           get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, PROBABILITY
+
+GearShifter = car.CarState.GearShifter
+
+class FrogPilotPlanner:
+  def __init__(self):
+    self.params_memory = Params("/dev/shm/params")
+
+    self.tracking_lead = False
+
+    self.acceleration_jerk = 0
+    self.danger_jerk = 0
+    self.speed_jerk = 0
+    self.v_cruise = 0
+
+    self.tracking_lead_mac = MovingAverageCalculator()
+
+  def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState):
+    self.lead_one = radarState.leadOne
+
+    v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS
+    v_ego = max(carState.vEgo, 0)
+    v_lead = self.lead_one.vLead
+
+    driving_gear = carState.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
+
+    lead_distance = self.lead_one.dRel
+    stopping_distance = STOP_DISTANCE
+
+    self.set_acceleration(controlsState, frogpilotCarState, v_cruise, v_ego)
+    self.set_follow_values(controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead)
+    self.set_lead_status(lead_distance, stopping_distance, v_ego)
+    self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego)
+
+  def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego):
+    if controlsState.experimentalMode:
+      self.max_accel = ACCEL_MAX
+    else:
+      self.max_accel = get_max_accel(v_ego)
+
+    if controlsState.experimentalMode:
+      self.min_accel = ACCEL_MIN
+    else:
+      self.min_accel = A_CRUISE_MIN
+
+  def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead):
+    self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(controlsState.personality)
+    self.t_follow = get_T_FOLLOW(controlsState.personality)
+
+    if self.tracking_lead:
+      self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead)
+    else:
+      self.acceleration_jerk = self.base_acceleration_jerk
+      self.danger_jerk = self.base_danger_jerk
+      self.speed_jerk = self.base_speed_jerk
+
+  def set_lead_status(self, lead_distance, stopping_distance, v_ego):
+    following_lead = self.lead_one.status and 1 < lead_distance < self.model_length + stopping_distance
+    following_lead &= v_ego > CRUISING_SPEED or self.tracking_lead
+
+    self.tracking_lead_mac.add_data(following_lead)
+    self.tracking_lead = self.tracking_lead_mac.get_moving_average() >= PROBABILITY
+
+  def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead):
+
+  def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego):
+    v_cruise_cluster = max(controlsState.vCruiseCluster, v_cruise) * CV.KPH_TO_MS
+    v_cruise_diff = v_cruise_cluster - v_cruise
+
+    v_ego_cluster = max(carState.vEgoCluster, v_ego)
+    v_ego_diff = v_ego_cluster - v_ego
+
+    targets = []
+    self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets]))
+
+  def publish(self, sm, pm):
+    frogpilot_plan_send = messaging.new_message('frogpilotPlan')
+    frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    frogpilotPlan = frogpilot_plan_send.frogpilotPlan
+
+    frogpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.acceleration_jerk)
+    frogpilotPlan.accelerationJerkStock = float(A_CHANGE_COST * self.base_acceleration_jerk)
+    frogpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.danger_jerk)
+    frogpilotPlan.speedJerk = float(J_EGO_COST * self.speed_jerk)
+    frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk)
+    frogpilotPlan.tFollow = float(self.t_follow)
+
+    frogpilotPlan.maxAcceleration = float(self.max_accel)
+    frogpilotPlan.minAcceleration = float(self.min_accel)
+
+    frogpilotPlan.vCruise = self.v_cruise
+
+    pm.send('frogpilotPlan', frogpilot_plan_send)
@@ -0,0 +1,2 @@
+CITY_SPEED_LIMIT = 25  # 55mph is typically the minimum speed for highways
+CRUISING_SPEED = 5     # Roughly the speed cars go when not touching the gas while in drive