From 9dcccf64f5cf6a7b76058a5e5b28b5b237ddc946 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 25 Nov 2022 15:10:38 +0900 Subject: [PATCH] feat(planning_launch): add steering rate limit params (#575) feat(planning_launch): add steering rate limit configs Signed-off-by: Makoto Kurihara Signed-off-by: Makoto Kurihara --- .../config/trajectory_follower/mpc_follower.param.yaml | 2 +- .../motion_velocity_smoother.param.yaml | 8 +++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/control_launch/config/trajectory_follower/mpc_follower.param.yaml b/control_launch/config/trajectory_follower/mpc_follower.param.yaml index 4085f7025b..5298e2b330 100644 --- a/control_launch/config/trajectory_follower/mpc_follower.param.yaml +++ b/control_launch/config/trajectory_follower/mpc_follower.param.yaml @@ -44,7 +44,7 @@ vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] diff --git a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index b2592316f6..a2b9255ef3 100644 --- a/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ b/planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -48,5 +48,11 @@ post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] + # steering angle rate limit parameters + max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + resample_ds: 0.1 # distance between trajectory points [m] + curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] + curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] + # system - over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point + over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point