diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8d09392fc4..e0d1421392 100644 --- a/cereal/custom.capnp +++ b/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 @2 :LongitudinalPersonalitySP; + struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; enabled @1 :Bool; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3f9d8245bd..268fba1e20 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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 log, 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): - if personality==log.LongitudinalPersonality.relaxed: +def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): + 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): - if personality==log.LongitudinalPersonality.relaxed: +def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): + 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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index fbc0097716..2ea4fa5ce8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math import numpy as np - +from cereal import custom import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.common.conversions import Conversions as CV @@ -163,9 +163,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 +195,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'] diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 60036c3082..0479b4267a 100755 --- a/selfdrive/controls/plannerd.py +++ b/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: diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index d336334a4e..6bf14cd02e 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -2,7 +2,7 @@ import math import os -from cereal import log, car +from cereal import log, car, custom import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.git import get_short_branch diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index b6a3348d5a..1c93ec0323 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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.LongitudinalPersonality.standard def params_thread(self, evt): while not evt.is_set():