fix(motion_velocity_smoother): prevent sudden deceleration in the lat…

…eral acceleration filtering process (tier4#1350)

* add filter to prevent sudden decel in latacc filter

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

* add null guard

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

* update readme

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

planning/motion_velocity_smoother/README.md

@@ -30,9 +30,11 @@ This function is used to approach near the obstacle or improve the accuracy of s
 #### Apply lateral acceleration limit
 
 It applies the velocity limit to decelerate at the curve.
-It calculate the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
+It calculates the velocity limit from the curvature of the reference trajectory and the maximum lateral acceleration `max_lateral_accel`.
 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.
+
 #### Resample trajectory
 
 It resamples the points on the reference trajectory with designated time interval.

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -157,7 +157,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node
 
   TrajectoryPoints calcTrajectoryVelocity(const TrajectoryPoints & input) const;
 
-  bool smoothVelocity(const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const;
+  bool smoothVelocity(
+    const TrajectoryPoints & input, const size_t input_closest,
+    TrajectoryPoints & traj_smoothed) const;
 
   std::tuple<double, double, InitializeType> calcInitialMotion(
     const TrajectoryPoints & input_traj, const size_t input_closest,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -75,7 +75,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
 
   boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
-    const TrajectoryPoints & input) const override;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+    [[maybe_unused]] const double a0,
+    [[maybe_unused]] const bool enable_smooth_limit) const override;
 
   void setParam(const Param & param);
   Param getParam() const;

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

@@ -60,7 +60,9 @@ class SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const = 0;
 
   virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
-    const TrajectoryPoints & input) const;
+    const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0,
+    [[maybe_unused]] const double a0 = 0.0,
+    [[maybe_unused]] const bool enable_smooth_limit = false) const;
 
   double getMaxAccel() const;
   double getMinDecel() const;

planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp

@@ -85,6 +85,10 @@ boost::optional<TrajectoryPoints> applyDecelFilterWithJerkConstraint(
 boost::optional<std::tuple<double, double, double, double>> updateStateWithJerkConstraint(
   const double v0, const double a0, const std::map<double, double> & jerk_profile, const double t);
 
+std::vector<double> calcVelocityProfileWithConstantJerkAndAccelerationLimit(
+  const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk,
+  const double acc_max, const double acc_min);
+
 }  // namespace trajectory_utils
 }  // namespace motion_velocity_smoother
 

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -478,7 +478,7 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
   }
 
   // Smoothing velocity
-  if (!smoothVelocity(*traj_extracted, output)) {
+  if (!smoothVelocity(*traj_extracted, *traj_extracted_closest, output)) {
     return prev_output_;
   }
 
@@ -493,10 +493,18 @@ TrajectoryPoints MotionVelocitySmootherNode::calcTrajectoryVelocity(
 }
 
 bool MotionVelocitySmootherNode::smoothVelocity(
-  const TrajectoryPoints & input, TrajectoryPoints & traj_smoothed) const
+  const TrajectoryPoints & input, const size_t input_closest,
+  TrajectoryPoints & traj_smoothed) const
 {
+  // Calculate initial motion for smoothing
+  double initial_vel{};
+  double initial_acc{};
+  InitializeType type{};
+  std::tie(initial_vel, initial_acc, type) = calcInitialMotion(input, input_closest, prev_output_);
+
   // Lateral acceleration limit
-  const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter(input);
+  const auto traj_lateral_acc_filtered =
+    smoother_->applyLateralAccelerationFilter(input, initial_vel, initial_acc, true);
   if (!traj_lateral_acc_filtered) {
     return false;
   }
@@ -505,13 +513,25 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   const auto traj_pre_resampled_closest = motion_utils::findNearestIndex(
     *traj_lateral_acc_filtered, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
     node_param_.delta_yaw_threshold);
+  if (!traj_pre_resampled_closest) {
+    RCLCPP_WARN(
+      get_logger(), "Cannot find closest waypoint for traj_lateral_acc_filtered trajectory");
+    return false;
+  }
   auto traj_resampled = smoother_->resampleTrajectory(
     *traj_lateral_acc_filtered, 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");
     return false;
   }
+  const auto traj_resampled_closest = motion_utils::findNearestIndex(
+    *traj_resampled, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
+    node_param_.delta_yaw_threshold);
+  if (!traj_resampled_closest) {
+    RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory");
+    return false;
+  }
 
   // Set 0[m/s] in the terminal point
   if (!traj_resampled->empty()) {
@@ -521,20 +541,6 @@ bool MotionVelocitySmootherNode::smoothVelocity(
   // Publish Closest Resample Trajectory Velocity
   publishClosestVelocity(*traj_resampled, current_pose_ptr_->pose, debug_closest_max_velocity_);
 
-  // Calculate initial motion for smoothing
-  double initial_vel{};
-  double initial_acc{};
-  InitializeType type{};
-  const auto traj_resampled_closest = motion_utils::findNearestIndex(
-    *traj_resampled, current_pose_ptr_->pose, std::numeric_limits<double>::max(),
-    node_param_.delta_yaw_threshold);
-  if (!traj_resampled_closest) {
-    RCLCPP_WARN(get_logger(), "Cannot find closest waypoint for resampled trajectory");
-    return false;
-  }
-  std::tie(initial_vel, initial_acc, type) =
-    calcInitialMotion(*traj_resampled, *traj_resampled_closest, prev_output_);
-
   // Clip trajectory from closest point
   TrajectoryPoints clipped;
   clipped.insert(

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -273,7 +273,8 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
 }
 
 boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter(
-  const TrajectoryPoints & input) const
+  const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+  [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
 {
   if (input.empty()) {
     return boost::none;

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -59,7 +59,8 @@ double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; }
 double SmootherBase::getMinJerk() const { return base_param_.min_jerk; }
 
 boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
-  const TrajectoryPoints & input) const
+  const TrajectoryPoints & input, [[maybe_unused]] const double v0,
+  [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const
 {
   if (input.empty()) {
     return boost::none;
@@ -102,6 +103,12 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
     static_cast<size_t>(std::round(base_param_.decel_distance_after_curve / points_interval));
   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>{};
+
   for (size_t i = 0; i < output->size(); ++i) {
     double curvature = 0.0;
     const size_t start = i > after_decel_index ? i - after_decel_index : 0;
@@ -111,6 +118,9 @@ 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) {
+      v_curvature_max = std::max(v_curvature_max, latacc_min_vel_arr.at(i));
+    }
     if (output->at(i).longitudinal_velocity_mps > v_curvature_max) {
       output->at(i).longitudinal_velocity_mps = v_curvature_max;
     }

planning/motion_velocity_smoother/src/trajectory_utils.cpp

@@ -639,5 +639,32 @@ boost::optional<std::tuple<double, double, double, double>> updateStateWithJerkC
   return {};
 }
 
+std::vector<double> calcVelocityProfileWithConstantJerkAndAccelerationLimit(
+  const TrajectoryPoints & trajectory, const double v0, const double a0, const double jerk,
+  const double acc_max, const double acc_min)
+{
+  if (trajectory.empty()) return {};
+
+  std::vector<double> velocities(trajectory.size());
+  velocities.at(0) = v0;
+  auto curr_v = v0;
+  auto curr_a = a0;
+
+  const auto intervals = calcTrajectoryIntervalDistance(trajectory);
+
+  if (intervals.size() + 1 != trajectory.size()) {
+    throw std::logic_error("interval calculation result has unexpected array size.");
+  }
+
+  for (size_t i = 0; i < intervals.size(); ++i) {
+    const auto t = intervals.at(i) / std::max(velocities.at(i), 1.0e-5);
+    curr_v = integ_v(curr_v, curr_a, jerk, t);
+    velocities.at(i + 1) = curr_v;
+    curr_a = std::clamp(integ_a(curr_a, jerk, t), acc_min, acc_max);
+  }
+
+  return velocities;
+}
+
 }  // namespace trajectory_utils
 }  // namespace motion_velocity_smoother