ff

cereal/custom.capnp

@@ -10,6 +10,13 @@ $Cxx.namespace("cereal");
 # DO rename the structs
 # DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
 
+enum LongitudinalPersonalitySP {
+  aggressive @0;
+  standard @1;
+  relaxed @2;
+  overtake @3;
+}
+
 struct ModularAssistiveDrivingSystem {
   state @0 :ModularAssistiveDrivingSystemState;
   enabled @1 :Bool;
@@ -27,6 +34,7 @@ struct ModularAssistiveDrivingSystem {
 
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
+  personality @1 :LongitudinalPersonalitySP;
 }
 
 struct ModelManagerSP @0xaedffd8f31e7b55d {
@@ -89,6 +97,8 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
 
+  personalityDEPRECATED @1 :LongitudinalPersonalitySP;
+
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
     enabled @1 :Bool;

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -2,7 +2,7 @@
 import os
 import time
 import numpy as np
-from cereal import log
+from cereal import custom
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
@@ -58,23 +58,23 @@ STOP_DISTANCE = 6.0
 CRUISE_MIN_ACCEL = -1.2
 CRUISE_MAX_ACCEL = 1.6
 
-def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard):
-  if personality==log.LongitudinalPersonality.relaxed:
+  if personality==custom.LongitudinalPersonalitySP.relaxed:
     return 1.0
-  elif personality==log.LongitudinalPersonality.standard:
+  elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.0
-  elif personality==log.LongitudinalPersonality.aggressive:
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 0.5
   else:
     raise NotImplementedError("Longitudinal personality not supported")
 
 
-def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard):
-  if personality==log.LongitudinalPersonality.relaxed:
+  if personality==custom.LongitudinalPersonalitySP.relaxed:
     return 1.75
-  elif personality==log.LongitudinalPersonality.standard:
+  elif personality==custom.LongitudinalPersonalitySP.standard:
     return 1.45
-  elif personality==log.LongitudinalPersonality.aggressive:
+  elif personality==custom.LongitudinalPersonalitySP.aggressive:
     return 1.25
   else:
     raise NotImplementedError("Longitudinal personality not supported")
@@ -274,7 +274,7 @@ 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):
+  def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
     jerk_factor = get_jerk_factor(personality)
     if self.mode == 'acc':
       a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
@@ -327,7 +327,7 @@ 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, x, v, a, j, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.conversions import Conversions as CV
@@ -163,9 +162,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     if force_slow_decel:
       v_cruise = 0.0
 
-    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveStateSP'].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.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveStateSP'].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)
@@ -193,7 +194,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')
 
-    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
+    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState', 'selfdriveStateSP'])
 
     longitudinalPlan = plan_send.longitudinalPlan
     longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']

selfdrive/controls/plannerd.py

@@ -19,7 +19,7 @@ def main():
   ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'selfdriveStateSP'],
                            poll='modelV2')
 
   while True:

selfdrive/selfdrived/selfdrived.py

@@ -32,7 +32,7 @@ REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
 IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
-LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
+LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in custom.LongitudinalPersonalitySP.schema.enumerants.items()}
 
 ThermalStatus = log.DeviceState.ThermalStatus
 State = log.SelfdriveState.OpenpilotState
@@ -475,7 +475,6 @@ class SelfdriveD(CruiseHelper):
     ss.state = self.state_machine.state
     ss.engageable = not self.events.contains(ET.NO_ENTRY)
     ss.experimentalMode = self.experimental_mode
-    ss.personality = self.personality
 
     ss.alertText1 = self.AM.current_alert.alert_text_1
     ss.alertText2 = self.AM.current_alert.alert_text_2
@@ -504,6 +503,7 @@ class SelfdriveD(CruiseHelper):
     mads.enabled = self.mads.enabled
     mads.active = self.mads.active
     mads.available = self.mads.enabled_toggle
+    ss_sp.personality = self.personality
 
     self.pm.send('selfdriveStateSP', ss_sp_msg)
 
@@ -532,7 +532,7 @@ class SelfdriveD(CruiseHelper):
     try:
       return int(self.params.get('LongitudinalPersonality'))
     except (ValueError, TypeError):
-      return log.LongitudinalPersonality.standard
+      return custom.LongitudinalPersonalitySP.standard
 
   def params_thread(self, evt):
     while not evt.is_set():

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -25,6 +25,7 @@ class Plant:
       Plant.car_state = messaging.pub_sock('carState')
       Plant.plan = messaging.sub_sock('longitudinalPlan')
       Plant.messaging_initialized = True
+      Plant.selfdrive_state_sp = messaging.pub_sock('selfdriveStateSP')
 
     self.v_lead_prev = 0.0
 
@@ -63,6 +64,7 @@ class Plant:
     radar = messaging.new_message('radarState')
     control = messaging.new_message('controlsState')
     ss = messaging.new_message('selfdriveState')
+    ss_sp = messaging.new_message('selfdriveStateSP')
     car_state = messaging.new_message('carState')
     lp = messaging.new_message('liveParameters')
     car_control = messaging.new_message('carControl')
@@ -118,7 +120,7 @@ class Plant:
 
     control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
     ss.selfdriveState.experimentalMode = self.e2e
-    ss.selfdriveState.personality = self.personality
+    ss_sp.selfdriveStateSP.personality = self.personality
     control.controlsState.forceDecel = self.force_decel
     car_state.carState.vEgo = float(self.speed)
     car_state.carState.standstill = bool(self.speed < 0.01)
@@ -131,6 +133,7 @@ class Plant:
           'carControl': car_control.carControl,
           'controlsState': control.controlsState,
           'selfdriveState': ss.selfdriveState,
+          'selfdriveStateSP': ss_sp.selfdriveStateSP,
           'liveParameters': lp.liveParameters,
           'modelV2': model.modelV2}
     self.planner.update(sm)