feat(motion_velocity_smoother): add steering rate limit while planning velocity

launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -45,7 +45,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]
 

launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml

@@ -9,7 +9,7 @@
 
     # curve parameters
     max_lateral_accel: 0.8             # max lateral acceleration limit [m/ss]
-    min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
+    min_curve_velocity: 0.5            # min velocity at lateral acceleration limit and steering angle rate limit [m/s]
     decel_distance_before_curve: 3.5   # slow speed distance before a curve for lateral acceleration limit
     decel_distance_after_curve: 2.0    # slow speed distance after a curve for lateral acceleration limit
 
@@ -47,5 +47,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

planning/motion_velocity_smoother/README.md

@@ -35,6 +35,10 @@ The velocity limit is set as not to fall under `min_curve_velocity`.
 
 Note: velocity limit that requests larger than `nominal.jerk` is not applied. In other words, even if a sharp curve is planned just in front of the ego, no deceleration is performed.
 
+#### Apply steering rate limit
+
+It calculates the desired steering angles of trajectory points. and it applies the steering rate limit. If the (`steering_angle_rate` > `max_steering_angle_rate`), it decreases the velocity of the trajectory point to acceptable velocity.
+
 #### Resample trajectory
 
 It resamples the points on the reference trajectory with designated time interval.
@@ -108,6 +112,7 @@ After the optimization, a resampling called `post resampling` is performed befor
 | `~/debug/trajectory_raw`                           | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug)                                                                          |
 | `~/debug/trajectory_external_velocity_limited`     | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug)                                                          |
 | `~/debug/trajectory_lateral_acc_filtered`          | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug)                                                |
+| `~/debug/trajectory_steering_rate_limited`         | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug)                                                 |
 | `~/debug/trajectory_time_resampled`                | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug)                                                                     |
 | `~/distance_to_stopline`                           | `std_msgs/Float32`                       | Distance to stop line from current ego pose (max 50 m) (for debug)                                        |
 | `~/stop_speed_exceeded`                            | `std_msgs/Bool`                          | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
@@ -189,6 +194,15 @@ After the optimization, a resampling called `post resampling` is performed befor
 | `post_sparse_dt`                    | `double` | resample time interval for sparse sampling [s]         | 0.1           |
 | `post_sparse_min_interval_distance` | `double` | minimum points-interval length for sparse sampling [m] | 1.0           |
 
+### Limit steering angle rate parameters
+
+| Name                             | Type     | Description                                                              | Default value |
+| :------------------------------- | :------- | :----------------------------------------------------------------------- | :------------ |
+| `max_steering_angle_rate`        | `double` | Maximum steering angle rate [degree/s]                                   | 40.0          |
+| `resample_ds`                    | `double` | Distance between trajectory points [m]                                   | 0.1           |
+| `curvature_threshold`            | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02          |
+| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m]                    | 1.0           |
+
 ### Weights for optimization
 
 #### JerkFiltered

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -81,10 +81,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   Odometry::ConstSharedPtr current_odometry_ptr_;  // current odometry
   Trajectory::ConstSharedPtr base_traj_raw_ptr_;   // current base_waypoints
   double external_velocity_limit_;                 // current external_velocity_limit
-
-  // maximum velocity with deceleration for external velocity limit
-  double max_velocity_with_deceleration_;
-  double external_velocity_limit_dist_{0.0};  // distance to set external velocity limit
+  double max_velocity_with_deceleration_;          // maximum velocity with deceleration
+                                                   // for external velocity limit
+  double external_velocity_limit_dist_{0.0};       // distance to set external velocity limit
+  double wheelbase_;                               // wheelbase
 
   TrajectoryPoints prev_output_;  // previously published trajectory
 
@@ -213,6 +213,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_vel_lim_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_latacc_filtered_;
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_steering_rate_limited_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_resampled_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_velocity_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_acc_;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -20,6 +20,7 @@
 #include "motion_velocity_smoother/trajectory_utils.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 
@@ -32,6 +33,7 @@ namespace motion_velocity_smoother
 {
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using vehicle_info_util::VehicleInfoUtil;
 
 class SmootherBase
 {
@@ -47,6 +49,13 @@ class SmootherBase
     double min_curve_velocity;           // min velocity at curve [m/s]
     double decel_distance_before_curve;  // distance before slow down for lateral acc at a curve
     double decel_distance_after_curve;   // distance after slow down for lateral acc at a curve
+    double max_steering_angle_rate;      // max steering angle rate [degree/s]
+    double wheel_base;                   // wheel base [m]
+    double sample_ds;                    // distance between trajectory points [m]
+    double curvature_threshold;  // look-up distance of Trajectory point for calculation of steering
+                                 // angle limit [m]
+    double curvature_calculation_distance;  // threshold steering degree limit to trigger
+                                            // steeringRateLimit [degree]
     resampling::ResampleParam resample_param;
   };
 
@@ -65,6 +74,8 @@ class SmootherBase
     [[maybe_unused]] const double a0 = 0.0,
     [[maybe_unused]] const bool enable_smooth_limit = false) const;
 
+  boost::optional<TrajectoryPoints> applySteeringRateLimit(const TrajectoryPoints & input) const;
+
   double getMaxAccel() const;
   double getMinDecel() const;
   double getMaxJerk() const;

planning/motion_velocity_smoother/package.xml

@@ -22,6 +22,7 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>libboost-dev</depend>
+  <depend>motion_common</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
   <depend>osqp_interface</depend>
@@ -31,6 +32,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
+  <depend>vehicle_info_util</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -19,6 +19,8 @@
 #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
 #include <algorithm>
 #include <chrono>
 #include <limits>
@@ -37,6 +39,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   using std::placeholders::_1;
 
   // set common params
+  const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
+  wheelbase_ = vehicle_info.wheel_base_m;
   initCommonParam();
   over_stop_velocity_warn_thr_ =
     declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0));
@@ -72,6 +76,10 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     default:
       throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
   }
+  // Initialize the wheelbase
+  auto p = smoother_->getBaseParam();
+  p.wheel_base = wheelbase_;
+  smoother_->setParam(p);
 
   // publishers, subscribers
   pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
@@ -93,7 +101,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
 
   // debug
-  publish_debug_trajs_ = declare_parameter("publish_debug_trajs", false);
+  publish_debug_trajs_ = declare_parameter("publish_debug_trajs", true);
   debug_closest_velocity_ = create_publisher<Float32Stamped>("~/closest_velocity", 1);
   debug_closest_acc_ = create_publisher<Float32Stamped>("~/closest_acceleration", 1);
   debug_closest_jerk_ = create_publisher<Float32Stamped>("~/closest_jerk", 1);
@@ -104,6 +112,8 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     create_publisher<Trajectory>("~/debug/trajectory_external_velocity_limited", 1);
   pub_trajectory_latacc_filtered_ =
     create_publisher<Trajectory>("~/debug/trajectory_lateral_acc_filtered", 1);
+  pub_trajectory_steering_rate_limited_ =
+    create_publisher<Trajectory>("~/debug/trajectory_steering_rate_limited", 1);
   pub_trajectory_resampled_ = create_publisher<Trajectory>("~/debug/trajectory_time_resampled", 1);
 
   // Wait for first self pose
@@ -170,6 +180,10 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
     update_param("min_interval_distance", p.resample_param.dense_min_interval_distance);
     update_param("sparse_resample_dt", p.resample_param.sparse_resample_dt);
     update_param("sparse_min_interval_distance", p.resample_param.sparse_min_interval_distance);
+    update_param("resample_ds", p.sample_ds);
+    update_param("curvature_threshold", p.curvature_threshold);
+    update_param("max_steering_angle_rate", p.max_steering_angle_rate);
+    update_param("curvature_calculation_distance", p.curvature_calculation_distance);
     smoother_->setParam(p);
   }
 
@@ -486,12 +500,23 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   const auto traj_lateral_acc_filtered =
     smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true);
   if (!traj_lateral_acc_filtered) {
+    RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered");
+
+    return false;
+  }
+
+  // Steering angle rate limit
+  const auto traj_steering_rate_limited =
+    smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
+  if (!traj_steering_rate_limited) {
+    RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited");
+
     return false;
   }
 
   // Resample trajectory with ego-velocity based interval distance
   auto traj_resampled = smoother_->resampleTrajectory(
-    *traj_lateral_acc_filtered, current_odometry_ptr_->twist.twist.linear.x,
+    *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x,
     current_pose_ptr_->pose, node_param_.ego_nearest_dist_threshold,
     node_param_.ego_nearest_yaw_threshold);
   if (!traj_resampled) {
@@ -550,6 +575,11 @@ bool MotionVelocitySmootherNode::smoothVelocity(
       if (is_reverse_) flipVelocity(tmp);
       pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp));
     }
+    {
+      auto tmp = *traj_steering_rate_limited;
+      if (is_reverse_) flipVelocity(tmp);
+      pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp));
+    }
 
     if (!debug_trajectories.empty()) {
       for (auto & debug_trajectory : debug_trajectories) {

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -14,17 +14,21 @@
 
 #include "motion_velocity_smoother/smoother/smoother_base.hpp"
 
+#include "motion_common/motion_common.hpp"
 #include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/tmp_conversion.hpp"
 #include "motion_velocity_smoother/resample.hpp"
 #include "motion_velocity_smoother/trajectory_utils.hpp"
+#include "tier4_autoware_utils/math/unit_conversion.hpp"
 
 #include <algorithm>
 #include <cmath>
 #include <vector>
 
 namespace motion_velocity_smoother
 {
+using vehicle_info_util::VehicleInfoUtil;
+
 SmootherBase::SmootherBase(rclcpp::Node & node)
 {
   auto & p = base_param_;
@@ -34,6 +38,10 @@ SmootherBase::SmootherBase(rclcpp::Node & node)
   p.max_jerk = node.declare_parameter("normal.max_jerk", 0.3);
   p.min_jerk = node.declare_parameter("normal.min_jerk", -0.1);
   p.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2);
+  p.sample_ds = node.declare_parameter("resample_ds", 0.5);
+  p.curvature_threshold = node.declare_parameter("curvature_threshold", 0.2);
+  p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.0);
+  p.curvature_calculation_distance = node.declare_parameter("curvature_calculation_distance", 1.0);
   p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve", 3.5);
   p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve", 0.0);
   p.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38);
@@ -114,6 +122,7 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
     }
     double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
     v_curvature_max = std::max(v_curvature_max, base_param_.min_curve_velocity);
+
     if (enable_smooth_limit) {
       if (i >= latacc_min_vel_arr.size()) return output;
       v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i));
@@ -125,4 +134,71 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   return output;
 }
 
+boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
+  const TrajectoryPoints & input) const
+{
+  if (input.empty()) {
+    return boost::none;
+  }
+
+  if (input.size() < 3) {
+    return boost::optional<TrajectoryPoints>(
+      input);  // cannot calculate the desired velocity. do nothing.
+  }
+  // Interpolate with constant interval distance for lateral acceleration calculation.
+  std::vector<double> out_arclength;
+  const auto traj_length = motion_utils::calcArcLength(input);
+  for (double s = 0; s < traj_length; s += base_param_.sample_ds) {
+    out_arclength.push_back(s);
+  }
+  const auto output_traj =
+    motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength);
+  auto output = motion_utils::convertToTrajectoryPointArray(output_traj);
+  output.back() = input.back();  // keep the final speed.
+
+  const size_t idx_dist = static_cast<size_t>(std::max(
+    static_cast<int>((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1));
+
+  // Calculate curvature assuming the trajectory points interval is constant
+  const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist);
+
+  for (size_t i = 0; i + 1 < output.size(); i++) {
+    if (fabs(curvature_v.at(i)) > base_param_.curvature_threshold) {
+      // calculate the just 2 steering angle
+      output.at(i).front_wheel_angle_rad = std::atan(base_param_.wheel_base * curvature_v.at(i));
+      output.at(i + 1).front_wheel_angle_rad =
+        std::atan(base_param_.wheel_base * curvature_v.at(i + 1));
+
+      const double mean_vel =
+        (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0;
+      const double dt =
+        std::max(base_param_.sample_ds / mean_vel, std::numeric_limits<double>::epsilon());
+      const double steering_diff =
+        fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad);
+      const double dt_steering =
+        steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate);
+
+      if (dt_steering > dt) {
+        const double target_mean_vel = (base_param_.sample_ds / dt_steering);
+        for (size_t k = 0; k < 2; k++) {
+          const double temp_vel =
+            output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel);
+          if (temp_vel < output.at(i + k).longitudinal_velocity_mps) {
+            output.at(i + k).longitudinal_velocity_mps = temp_vel;
+          } else {
+            if (target_mean_vel < output.at(i + k).longitudinal_velocity_mps) {
+              output.at(i + k).longitudinal_velocity_mps = target_mean_vel;
+            }
+          }
+          if (output.at(i + k).longitudinal_velocity_mps < base_param_.min_curve_velocity) {
+            output.at(i + k).longitudinal_velocity_mps = base_param_.min_curve_velocity;
+          }
+        }
+      }
+    }
+  }
+
+  return output;
+}
+
 }  // namespace motion_velocity_smoother