Sane curvature rate limit in plannerd (#20366)
* safety limit * should be in RADs * add raw * update refsalbatross
parent
e3ad952902
commit
27035131cc
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit 97bfe99980a3ef58e07eb8006ac5e59bf83ad986
|
||||
Subproject commit 0956034a30acbb6bad8916839174429b286e346b
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -1 +1 @@
|
|||
7ad3a680a6337494b9e79c404d27a99abf4fdf8a
|
||||
68856f382a9bc423ad6997b2fe0d584ba4edb4ae
|
Loading…
Reference in New Issue