|
|
|
@ -2,7 +2,7 @@ import os |
|
|
|
|
import math |
|
|
|
|
import numpy as np |
|
|
|
|
from common.realtime import sec_since_boot, DT_MDL |
|
|
|
|
from common.numpy_fast import interp |
|
|
|
|
from common.numpy_fast import interp, clip |
|
|
|
|
from selfdrive.swaglog import cloudlog |
|
|
|
|
from selfdrive.controls.lib.lateral_mpc import libmpc_py |
|
|
|
|
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATION_RADIUS |
|
|
|
@ -19,6 +19,9 @@ LOG_MPC = os.environ.get('LOG_MPC', False) |
|
|
|
|
|
|
|
|
|
LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS |
|
|
|
|
LANE_CHANGE_TIME_MAX = 10. |
|
|
|
|
# this corresponds to 80deg/s and 20deg/s steering angle in a toyota corolla |
|
|
|
|
MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992] |
|
|
|
|
MAX_CURVATURE_RATE_SPEEDS = [0, 35] |
|
|
|
|
|
|
|
|
|
DESIRES = { |
|
|
|
|
LaneChangeDirection.none: { |
|
|
|
@ -76,7 +79,9 @@ class LateralPlanner(): |
|
|
|
|
self.cur_state[0].curvature = 0.0 |
|
|
|
|
|
|
|
|
|
self.desired_curvature = 0.0 |
|
|
|
|
self.safe_desired_curvature = 0.0 |
|
|
|
|
self.desired_curvature_rate = 0.0 |
|
|
|
|
self.safe_desired_curvature_rate = 0.0 |
|
|
|
|
|
|
|
|
|
def update(self, sm, CP): |
|
|
|
|
v_ego = sm['carState'].vEgo |
|
|
|
@ -187,6 +192,13 @@ class LateralPlanner(): |
|
|
|
|
|
|
|
|
|
self.desired_curvature = next_curvature |
|
|
|
|
self.desired_curvature_rate = next_curvature_rate |
|
|
|
|
max_curvature_rate = interp(v_ego, MAX_CURVATURE_RATE_SPEEDS, MAX_CURVATURE_RATES) |
|
|
|
|
self.safe_desired_curvature_rate = clip(self.desired_curvature_rate, |
|
|
|
|
-max_curvature_rate, |
|
|
|
|
max_curvature_rate) |
|
|
|
|
self.safe_desired_curvature = clip(self.desired_curvature, |
|
|
|
|
self.safe_desired_curvature - max_curvature_rate/DT_MDL, |
|
|
|
|
self.safe_desired_curvature + max_curvature_rate/DT_MDL) |
|
|
|
|
|
|
|
|
|
# Check for infeasable MPC solution |
|
|
|
|
mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) |
|
|
|
@ -214,8 +226,10 @@ class LateralPlanner(): |
|
|
|
|
plan_send.lateralPlan.rProb = float(self.LP.rll_prob) |
|
|
|
|
plan_send.lateralPlan.dProb = float(self.LP.d_prob) |
|
|
|
|
|
|
|
|
|
plan_send.lateralPlan.curvature = float(self.desired_curvature) |
|
|
|
|
plan_send.lateralPlan.curvatureRate = float(self.desired_curvature_rate) |
|
|
|
|
plan_send.lateralPlan.rawCurvature = float(self.desired_curvature) |
|
|
|
|
plan_send.lateralPlan.rawCurvatureRate = float(self.desired_curvature_rate) |
|
|
|
|
plan_send.lateralPlan.curvature = float(self.safe_desired_curvature) |
|
|
|
|
plan_send.lateralPlan.curvatureRate = float(self.safe_desired_curvature_rate) |
|
|
|
|
|
|
|
|
|
plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid) |
|
|
|
|
|
|
|
|
|