Sane curvature rate limit in plannerd (#20366)

* safety limit

* should be in RADs

* add raw

* update refs
old-commit-hash: 27035131cc97afdf19f682911f580bd689493794

selfdrive/controls/lib/lateral_planner.py

@@ -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)
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-7ad3a680a6337494b9e79c404d27a99abf4fdf8a
+68856f382a9bc423ad6997b2fe0d584ba4edb4ae