#!/usr/bin/env python3 import math import numpy as np from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params from cereal import log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import T_IDXS from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX 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 V_CRUISE_MAX, CONTROL_N, get_speed_error from openpilot.system.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController from openpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController from openpilot.selfdrive.controls.lib.accel_controller import AccelController from openpilot.selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController 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] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] def get_max_accel(v_ego): return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration this should avoid accelerating when losing the target in turns """ # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel # The lookup table for turns should also be updated if we do this a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) return [a_target[0], min(a_target[1], a_x_allowed)] class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0): # mapd self.cruise_source = 'cruise' self.vision_turn_controller = VisionTurnController(CP) self.speed_limit_controller = SpeedLimitController() self.turn_speed_controller = TurnSpeedController() self.accel_controller = AccelController() self.dynamic_endtoend_controller = DynamicEndtoEndController() self.CP = CP self.mpc = LongitudinalMpc() self.fcw = False self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL) self.v_model_error = 0.0 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 self.params = Params() self.param_read_counter = 0 self.read_param() self.personality = log.LongitudinalPersonality.standard self.dp_long_use_df_tune = False self.dp_long_taco = self.params.get_bool('dp_long_taco') def read_param(self): try: self.personality = int(self.params.get('LongitudinalPersonality')) except (ValueError, TypeError): self.personality = log.LongitudinalPersonality.standard self.dp_long_use_df_tune = self.params.get_bool('dp_long_use_df_tune') @staticmethod def parse_model(model_msg, model_error, v_ego, taco=False): if (len(model_msg.position.x) == 33 and len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error a = np.interp(T_IDXS_MPC, 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)) # rick - taco tune if taco: max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0]) curvatures = np.interp(T_IDXS_MPC, T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0) max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0 v = np.minimum(max_v, v) return x, v, a, j def update(self, sm): if self.param_read_counter % 50 == 0: self.read_param() if self.param_read_counter % 100 == 0: self.accel_controller.set_profile(self.params.get("dp_long_accel_profile", encoding='utf-8')) self.vision_turn_controller.set_enabled(self.params.get_bool("dp_mapd_vision_turn_control")) self.dynamic_endtoend_controller.set_enabled(self.params.get_bool("dp_long_de2e")) self.param_read_counter += 1 if self.dynamic_endtoend_controller.is_enabled(): self.mpc.mode = self.dynamic_endtoend_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) else: self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) 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] # dp - override accel using dp_long_accel_profile if self.accel_controller.is_enabled(): # get min, max from accel controller min_limit, max_limit = self.accel_controller.get_accel_limits(v_ego, accel_limits) if self.mpc.mode == 'acc': # voacc car, just give it max min (-1.2) so I can brake harder if self.CP.radarUnavailable: accel_limits = [A_CRUISE_MIN, max_limit] else: accel_limits = [min_limit, max_limit] # recalculate limit turn according to the new min, max accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) else: # blended, just give it max min (-3.5) and max from accel controller accel_limits = accel_limits_turns = [ACCEL_MIN, max_limit] 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 = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) # Compute model v_ego error self.v_model_error = get_speed_error(sm['modelV2'], v_ego) # Get acceleration and active solutions for custom long mpc. self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(not reset_state, self.v_desired_filter.x, self.a_desired, v_cruise, sm) if force_slow_decel: v_cruise_sol = 0.0 # clip limits, cannot init MPC outside of bounds accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol) 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_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, v_ego, taco=self.dp_long_taco) self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, use_df_tune=self.dp_long_use_df_tune) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], 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") # Interpolate 0.05 seconds and save as starting point for next iteration a_prev = self.a_desired self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0 def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) longitudinalPlan = plan_send.longitudinalPlan longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] 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.fcw = self.fcw longitudinalPlan.solverExecutionTime = self.mpc.solve_time longitudinalPlan.personality = self.personality pm.send('longitudinalPlan', plan_send) # dp - extension plan_ext_send = messaging.new_message('longitudinalPlanExt') longitudinalPlanExt = plan_ext_send.longitudinalPlanExt longitudinalPlanExt.visionTurnControllerState = self.vision_turn_controller.state longitudinalPlanExt.visionTurnSpeed = float(self.vision_turn_controller.v_turn) longitudinalPlanExt.speedLimitControlState = self.speed_limit_controller.state longitudinalPlanExt.speedLimit = float(self.speed_limit_controller.speed_limit) longitudinalPlanExt.speedLimitOffset = float(self.speed_limit_controller.speed_limit_offset) longitudinalPlanExt.distToSpeedLimit = float(self.speed_limit_controller.distance) longitudinalPlanExt.isMapSpeedLimit = bool(self.speed_limit_controller.source == SpeedLimitResolver.Source.map_data) longitudinalPlanExt.turnSpeedControlState = self.turn_speed_controller.state longitudinalPlanExt.turnSpeed = float(self.turn_speed_controller.speed_limit) longitudinalPlanExt.distToTurn = float(self.turn_speed_controller.distance) longitudinalPlanExt.turnSign = int(self.turn_speed_controller.turn_sign) longitudinalPlanExt.dpE2EIsBlended = self.mpc.mode == 'blended' longitudinalPlanExt.de2eIsEnabled = self.dynamic_endtoend_controller.is_enabled() longitudinalPlanExt.longitudinalPlanExtSource = self.mpc.source if self.mpc.source != 'cruise' else self.cruise_source pm.send('longitudinalPlanExt', plan_ext_send) # mapd def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm): # Update controllers self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm) self.speed_limit_controller.update(enabled, v_ego, a_ego, v_cruise, sm['carState'].gasPressed) self.turn_speed_controller.update(enabled, v_ego, a_ego, sm) # Pick solution with lowest velocity target. a_solutions = {'cruise': float("inf")} v_solutions = {'cruise': v_cruise} if self.vision_turn_controller.is_active: a_solutions['turn'] = self.vision_turn_controller.a_target v_solutions['turn'] = self.vision_turn_controller.v_turn if self.speed_limit_controller.is_active: a_solutions['limit'] = self.speed_limit_controller.a_target v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted if self.turn_speed_controller.is_active: a_solutions['turnlimit'] = self.turn_speed_controller.a_target v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit source = min(v_solutions, key=v_solutions.get) return source, a_solutions[source], v_solutions[source]