From c07a1ad57bb884a5cd5c0d67776c9e3e54cd33af Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Tue, 16 Mar 2021 15:00:03 -0700
Subject: [PATCH] Sane curvature rate limit in plannerd (#20366)

* safety limit

* should be in RADs

* add raw

* update refs
old-commit-hash: 27035131cc97afdf19f682911f580bd689493794
---
 cereal                                    |  2 +-
 selfdrive/controls/lib/lateral_planner.py | 20 +++++++++++++++++---
 selfdrive/test/process_replay/ref_commit  |  2 +-
 3 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/cereal b/cereal
index 97bfe99980..0956034a30 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 97bfe99980a3ef58e07eb8006ac5e59bf83ad986
+Subproject commit 0956034a30acbb6bad8916839174429b286e346b
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 29d540a366..0aa2da55a9 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/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)
 
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 7f90850cac..cbc93dc6ab 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-7ad3a680a6337494b9e79c404d27a99abf4fdf8a
\ No newline at end of file
+68856f382a9bc423ad6997b2fe0d584ba4edb4ae
\ No newline at end of file