feat(motion_velocity_smoother): add steering rate limit while plannin…

…g velocity (autowarefoundation#1071)

function added,


not turning


fix the always positive curvature problem


added lower velocity limit


added vehicle parameters


functions created

Signed-off-by: Berkay <berkay@leodrive.ai>

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

@@ -46,8 +46,8 @@
     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_lim_deg: 40.0              # steering angle limit [deg]
-    steer_rate_lim_dps: 600.0        # steering angle rate limit [deg/s]
-    acceleration_limit: 2.0          # acceleration limit for trajectory velocity modification [m/ss]
+    steer_rate_lim_dps: 5.0        # steering angle rate limit [deg/s]
+    acceleration_limit: 0.9          # acceleration limit for trajectory velocity modification [m/ss]
     velocity_time_constant: 0.3      # velocity dynamics time constant  for trajectory velocity modification [s]
 
     # -- lowpass filter for noise reduction --

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

@@ -59,12 +59,12 @@
     emergency_jerk: -3.0
 
     # acceleration limit
-    max_acc: 3.0
-    min_acc: -5.0
+    max_acc: 0.9
+    min_acc: -0.9
 
     # jerk limit
-    max_jerk: 2.0
-    min_jerk: -5.0
+    max_jerk: 0.6
+    min_jerk: -0.6
 
     # pitch
     use_trajectory_for_pitch_calculation: false

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

@@ -3,13 +3,13 @@
     # constraints param for normal driving
     normal:
       min_acc: -0.5         # min deceleration [m/ss]
-      max_acc: 1.0          # max acceleration [m/ss]
-      min_jerk: -0.5        # min jerk [m/sss]
-      max_jerk: 1.0         # max jerk [m/sss]
+      max_acc: 0.5          # max acceleration [m/ss]
+      min_jerk: -0.4        # min jerk [m/sss]
+      max_jerk: 0.4         # max jerk [m/sss]
 
     # constraints to be observed
     limit:
-      min_acc: -2.5         # min deceleration limit [m/ss]
-      max_acc: 1.0          # max acceleration limit [m/ss]
-      min_jerk: -1.5        # min jerk limit [m/sss]
-      max_jerk: 1.5         # max jerk limit [m/sss]
+      min_acc: -0.9         # min deceleration limit [m/ss]
+      max_acc: 0.9          # max acceleration limit [m/ss]
+      min_jerk: -0.6        # min jerk limit [m/sss]
+      max_jerk: 0.6         # max jerk limit [m/sss]

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

@@ -8,8 +8,8 @@
     margin_to_insert_external_velocity_limit: 0.3   #  margin distance to insert external velocity limit [m]
 
     # 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]
+    max_lateral_accel: 0.3             # max lateral acceleration limit [m/ss]
+    min_curve_velocity: 0.5            # 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
 
@@ -32,7 +32,7 @@
     # resampling parameters for optimization
     max_trajectory_length: 200.0        # max trajectory length for resampling [m]
     min_trajectory_length: 150.0        # min trajectory length for resampling [m]
-    resample_time: 5.0                 # resample total time for dense sampling [s]
+    resample_time: 5.0                  # resample total time for dense sampling [s]
     dense_dt: 0.1                       # resample time interval for dense sampling [s]
     dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
     sparse_dt: 0.5                      # resample time interval for sparse sampling [s]
@@ -41,11 +41,14 @@
     # resampling parameters for post process
     post_max_trajectory_length: 300.0        # max trajectory length for resampling [m]
     post_min_trajectory_length: 30.0         # min trajectory length for resampling [m]
-    post_resample_time: 10.0                  # resample total time for dense sampling [s]
+    post_resample_time: 10.0                 # resample total time for dense sampling [s]
     post_dense_dt: 0.1                       # resample time interval for dense sampling [s]
     post_dense_min_interval_distance: 0.1    # minimum points-interval length for dense sampling [m]
     post_sparse_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]
 
+    # vehicle model parameters
+    max_steering_angle_rate: 0.0872665             # steering angle rate [rad/s]
+
     # 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

map/map_loader/config/lanelet2_map_loader.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    lanelet2_map_projector_type: MGRS   # Options: MGRS, UTM
-    latitude: 40.816617984672746        # Latitude of map_origin, using in UTM
-    longitude: 29.360491808334285       # Longitude of map_origin, using in UTM
+    lanelet2_map_projector_type: UTM   # Options: MGRS, UTM
+    latitude: 40.81187906        # Latitude of map_origin, using in UTM
+    longitude: 29.35810110       # Longitude of map_origin, using in UTM
 
     center_line_resolution: 5.0         # [m]

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -82,6 +82,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   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
   boost::optional<TrajectoryPoint> prev_closest_point_{};  // previous trajectory point
@@ -197,6 +198,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

@@ -22,6 +22,7 @@
 #include "tier4_autoware_utils/trajectory/trajectory.hpp"
 
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include "boost/optional.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,7 +49,9 @@ 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
-    resampling::ResampleParam resample_param;
+    double max_steering_angle_rate;       // max steering angle rate [degree/s]
+    double wheel_base;                   // wheel base [m]
+      resampling::ResampleParam resample_param;
   };
 
   explicit SmootherBase(rclcpp::Node & node);
@@ -62,6 +66,10 @@ class SmootherBase
   virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
     const TrajectoryPoints & input) const;
 
+  boost::optional<TrajectoryPoints> applySteeringRateLimit(
+    const TrajectoryPoints & input) const;
+
+
   double getMaxAccel() const;
   double getMinDecel() const;
   double getMaxJerk() const;
@@ -72,6 +80,10 @@ class SmootherBase
 
 protected:
   BaseParam base_param_;
+  const double points_interval = 0.1; // constant interval distance of trajectory
+                                      // for lateral acceleration calculation and
+                                      // limit steering rate. [m]
+
 };
 }  // namespace motion_velocity_smoother
 

planning/motion_velocity_smoother/package.xml

@@ -26,6 +26,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
+  <depend>vehicle_info_util</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -18,6 +18,7 @@
 #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
 #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>
@@ -37,6 +38,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,8 +75,12 @@ 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
+    // publishers, subscribers
   pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
   pub_velocity_limit_ = create_publisher<VelocityLimit>(
     "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
@@ -93,7 +100,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 +111,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
@@ -167,6 +176,7 @@ 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("max_steering_angle_rate", p.max_steering_angle_rate);
     smoother_->setParam(p);
   }
 
@@ -498,15 +508,27 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   // Lateral acceleration limit
   const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input);
   if (!traj_lateral_acc_filtered) {
+    RCLCPP_WARN(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_WARN(get_logger(), "Fail to do traj_steering_rate_limited");
+
     return false;
   }
+  RCLCPP_WARN(get_logger(), "AAAAAAAAA CREATED");
+
 
   // Resample trajectory with ego-velocity based interval distance
   const auto traj_pre_resampled_closest = tier4_autoware_utils::findNearestIndex(
-    *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    *traj_steering_rate_limited, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
     node_param_.delta_yaw_threshold);
   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,
     *traj_pre_resampled_closest);
   if (!traj_resampled) {
     RCLCPP_WARN(get_logger(), "Fail to do resampling before the optimization");
@@ -568,6 +590,8 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   if (publish_debug_trajs_) {
     pub_trajectory_latacc_filtered_->publish(
       toTrajectoryMsg(*traj_lateral_acc_filtered, base_traj_raw_ptr_->header));
+    pub_trajectory_steering_rate_limited_->publish(
+      toTrajectoryMsg(*traj_steering_rate_limited, base_traj_raw_ptr_->header));
     pub_trajectory_resampled_->publish(
       toTrajectoryMsg(*traj_resampled, base_traj_raw_ptr_->header));