move personality to controlsState, since eventually it won't be exclusive to long planner

more

bump
pull/31792/head
Shane Smiskol 2 years ago
parent 45298d9a52
commit 06480657a2
  1. 2
      cereal
  2. 12
      selfdrive/controls/controlsd.py
  3. 35
      selfdrive/controls/lib/longitudinal_planner.py
  4. 3
      selfdrive/controls/plannerd.py
  5. 3
      selfdrive/test/longitudinal_maneuvers/plant.py

@ -1 +1 @@
Subproject commit 93d3df3210a8c4f4c7e6f45950f73dc8a2f09a2c
Subproject commit ae018910975883dfe70700fe161092ef62050fc2

@ -146,6 +146,7 @@ class Controls:
self.steer_limited = False
self.desired_curvature = 0.0
self.experimental_mode = False
self.personality = log.LongitudinalPersonality.standard
self.v_cruise_helper = VCruiseHelper(self.CP)
self.recalibrating_seen = False
@ -650,6 +651,12 @@ class Controls:
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
self.personality = (self.personality - 1) % 3
self.params.put('LongitudinalPersonality', str(self.personality))
return CC, lac_log
def publish_logs(self, CS, start_time, CC, lac_log):
@ -769,6 +776,7 @@ class Controls:
controlsState.forceDecel = bool(force_decel)
controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter
controlsState.experimentalMode = self.experimental_mode
controlsState.personality = self.personality
lat_tuning = self.CP.lateralTuning.which()
if self.joystick_mode:
@ -825,6 +833,10 @@ class Controls:
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
try:
self.personality = int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
self.personality = log.LongitudinalPersonality.standard
if self.CP.notCar:
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
time.sleep(0.1)

@ -2,8 +2,6 @@
import math
import numpy as np
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.params import Params
from cereal import car, log
import cereal.messaging as messaging
from openpilot.common.conversions import Conversions as CV
@ -17,8 +15,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
from openpilot.common.swaglog import cloudlog
ButtonType = car.CarState.ButtonEvent.Type
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@ -63,19 +59,6 @@ class LongitudinalPlanner:
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
self.solverExecutionTime = 0.0
self.params = Params()
self.param_read_counter = 0
self.personality = log.LongitudinalPersonality.standard
self.read_param()
def read_param(self):
try:
self.personality = int(self.params.get('LongitudinalPersonality'))
except (ValueError, TypeError):
self.personality = log.LongitudinalPersonality.standard
def write_param(self):
self.params.put('LongitudinalPersonality', str(self.personality))
@staticmethod
def parse_model(model_msg, model_error):
@ -93,18 +76,7 @@ class LongitudinalPlanner:
j = np.zeros(len(T_IDXS_MPC))
return x, v, a, j
def update(self, sm, carState_sock):
if self.param_read_counter % 50 == 0:
self.read_param()
# decrement personality on distance button press
if self.CP.openpilotLongitudinalControl:
for m in messaging.drain_sock(carState_sock, wait_for_one=False):
if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in m.carState.buttonEvents):
self.personality = (self.personality - 1) % 3
self.write_param()
self.param_read_counter += 1
def update(self, sm):
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
v_ego = sm['carState'].vEgo
@ -143,11 +115,11 @@ 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=self.personality)
self.mpc.set_weights(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=self.personality)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
@ -183,6 +155,5 @@ class LongitudinalPlanner:
longitudinalPlan.fcw = self.fcw
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
longitudinalPlan.personality = self.personality
pm.send('longitudinalPlan', plan_send)

@ -30,12 +30,11 @@ def plannerd_thread():
pm = messaging.PubMaster(['longitudinalPlan', 'uiPlan'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2'],
poll='modelV2', ignore_avg_freq=['radarState'])
carState_sock = messaging.sub_sock('carState')
while True:
sm.update()
if sm.updated['modelV2']:
longitudinal_planner.update(sm, carState_sock)
longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
publish_ui_plan(sm, pm, longitudinal_planner)

@ -50,7 +50,6 @@ class Plant:
from openpilot.selfdrive.car.honda.interface import CarInterface
self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.CIVIC), init_v=self.speed)
self.carState_sock = messaging.sub_sock('carState')
@property
def current_time(self):
@ -122,7 +121,7 @@ class Plant:
'carState': car_state.carState,
'controlsState': control.controlsState,
'modelV2': model.modelV2}
self.planner.update(sm, self.carState_sock)
self.planner.update(sm)
self.speed = self.planner.v_desired_filter.x
self.acceleration = self.planner.a_desired
self.speeds = self.planner.v_desired_trajectory.tolist()

Loading…
Cancel
Save