feat(motion_velocity_smoother): tunable deceleration limit for curve … (autowarefoundation#2278)

feat(motion_velocity_smoother): tunable deceleration limit for curve deceleration

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Author: TakaHoribe

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

@@ -12,6 +12,7 @@
     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
+    min_decel_for_lateral_acc_lim_filter: -2.5  # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss]
 
     # engage & replan parameters
     replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]

planning/motion_velocity_smoother/README.md

@@ -138,12 +138,13 @@ After the optimization, a resampling called `post resampling` is performed befor
 
 ### Curve parameters
 
-| Name                          | Type     | Description                                                            | Default value |
-| :---------------------------- | :------- | :--------------------------------------------------------------------- | :------------ |
-| `max_lateral_accel`           | `double` | Max lateral acceleration limit [m/ss]                                  | 0.5           |
-| `min_curve_velocity`          | `double` | Min velocity at lateral acceleration limit [m/ss]                      | 2.74          |
-| `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5           |
-| `decel_distance_after_curve`  | `double` | Distance to slowdown after a curve for lateral acceleration limit [m]  | 2.0           |
+| Name                                   | Type     | Description                                                                                                                                                                                                  | Default value |
+| :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
+| `max_lateral_accel`                    | `double` | Max lateral acceleration limit [m/ss]                                                                                                                                                                        | 0.5           |
+| `min_curve_velocity`                   | `double` | Min velocity at lateral acceleration limit [m/ss]                                                                                                                                                            | 2.74          |
+| `decel_distance_before_curve`          | `double` | Distance to slowdown before a curve for lateral acceleration limit [m]                                                                                                                                       | 3.5           |
+| `decel_distance_after_curve`           | `double` | Distance to slowdown after a curve for lateral acceleration limit [m]                                                                                                                                        | 2.0           |
+| `min_decel_for_lateral_acc_lim_filter` | `double` | Deceleration limit to avoid sudden braking by the lateral acceleration filter [m/ss]. Strong limitation degrades the deceleration response to the appearance of sharp curves due to obstacle avoidance, etc. | -2.5          |
 
 ### Engage & replan parameters
 

planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml

@@ -12,6 +12,7 @@
     min_curve_velocity: 2.74           # min velocity at lateral acceleration limit [m/ss]
     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
+    min_decel_for_lateral_acc_lim_filter: -2.5  # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss]
 
     # engage & replan parameters
     replan_vel_deviation: 5.53         # velocity deviation to replan initial velocity [m/s]

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

@@ -45,8 +45,10 @@ class SmootherBase
     double stop_decel;  // deceleration at a stop point [m/s2] <= 0
     double max_jerk;
     double min_jerk;
-    double max_lateral_accel;            // max lateral acceleration [m/ss] > 0
-    double min_curve_velocity;           // min velocity at curve [m/s]
+    double max_lateral_accel;                     // max lateral acceleration [m/ss] > 0
+    double min_decel_for_lateral_acc_lim_filter;  // deceleration limit applied in the lateral
+                                                  // acceleration filter to avoid sudden braking.
+    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]

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -37,6 +37,8 @@ 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.min_decel_for_lateral_acc_lim_filter =
+    node.declare_parameter("min_decel_for_lateral_acc_lim_filter", -2.5);
   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);
@@ -106,10 +108,10 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
   const double max_lateral_accel_abs = std::fabs(base_param_.max_lateral_accel);
 
   const auto latacc_min_vel_arr =
-    enable_smooth_limit
-      ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit(
-          output, v0, a0, base_param_.min_jerk, base_param_.max_accel, base_param_.min_decel)
-      : std::vector<double>{};
+    enable_smooth_limit ? trajectory_utils::calcVelocityProfileWithConstantJerkAndAccelerationLimit(
+                            output, v0, a0, base_param_.min_jerk, base_param_.max_accel,
+                            base_param_.min_decel_for_lateral_acc_lim_filter)
+                        : std::vector<double>{};
 
   for (size_t i = 0; i < output.size(); ++i) {
     double curvature = 0.0;