feat(avoidance): limit acceleration during avoidance maneuver

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::helper::avoidance
@@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
 using motion_utils::calcDecelDistWithJerkAndAccConstraints;
+using motion_utils::calcLongitudinalOffsetPose;
+using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -61,6 +65,11 @@ class AvoidanceHelper
 
   geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; }
 
+  geometry_msgs::msg::Point getEgoPosition() const
+  {
+    return data_->self_odometry->pose.pose.position;
+  }
+
   static size_t getConstraintsMapIndex(const double velocity, const std::vector<double> & values)
   {
     const auto itr = std::find_if(
@@ -262,6 +271,20 @@ class AvoidanceHelper
     return *itr;
   }
 
+  std::pair<double, double> getDistanceToAccelEndPoint(const PathWithLaneId & path)
+  {
+    updateAccelEndPoint(path);
+
+    if (!max_v_point_.has_value()) {
+      return std::make_pair(0.0, std::numeric_limits<double>::max());
+    }
+
+    const auto start_idx = data_->findEgoIndex(path.points);
+    const auto distance =
+      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    return std::make_pair(distance, max_v_point_.value().second);
+  }
+
   double getFeasibleDecelDistance(
     const double target_velocity, const bool use_hard_constraints = true) const
   {
@@ -367,6 +390,37 @@ class AvoidanceHelper
     return true;
   }
 
+  void updateAccelEndPoint(const PathWithLaneId & path)
+  {
+    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
+    if (a_now < 0.0) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (max_v_point_.has_value()) {
+      return;
+    }
+
+    const auto & p = parameters_;
+    const auto & v_max = data_->parameters.max_vel;
+    const auto v0 = getEgoSpeed() + p->buf_slow_down_speed;
+    const auto t_accel = (v_max - v0) / p->max_acceleration;
+    const auto x_accel = v0 * t_accel + 0.5 * p->max_acceleration * t_accel * t_accel;
+
+    const auto point = calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_accel);
+    if (point.has_value()) {
+      max_v_point_ = std::make_pair(point.value(), v_max);
+    }
+
+    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto t_end =
+      (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
+    const auto v_end = v0 + p->max_acceleration * t_end;
+
+    max_v_point_ = std::make_pair(getPose(path.points.back()), v_end);
+  }
+
   void reset()
   {
     prev_reference_path_ = PathWithLaneId();
@@ -417,6 +471,8 @@ class AvoidanceHelper
   ShiftedPath prev_linear_shift_path_;
 
   lanelet::ConstLanelets prev_driving_lanes_;
+
+  std::optional<std::pair<Pose, double>> max_v_point_;
 };
 }  // namespace behavior_path_planner::helper::avoidance
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -235,6 +235,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
+  /**
+   * @brief insert max velocity in output path to limit acceleration.
+   * @param target path.
+   */
+  void insertAvoidanceVelocity(ShiftedPath & shifted_path) const;
+
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.

planning/behavior_path_avoidance_module/src/manager.cpp

@@ -146,6 +146,16 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     }
   }
 
+  {
+    const std::string ns = "avoidance.constraints.longitudinal.";
+
+    updateParam<double>(parameters, ns + "nominal_deceleration", p->nominal_deceleration);
+    updateParam<double>(parameters, ns + "nominal_jerk", p->nominal_jerk);
+    updateParam<double>(parameters, ns + "max_deceleration", p->max_deceleration);
+    updateParam<double>(parameters, ns + "max_jerk", p->max_jerk);
+    updateParam<double>(parameters, ns + "max_acceleration", p->max_acceleration);
+  }
+
   {
     const std::string ns = "avoidance.shift_line_pipeline.";
     updateParam<double>(

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -686,6 +686,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
   }
 
   insertPrepareVelocity(path);
+  insertAvoidanceVelocity(path);
 
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
@@ -1706,6 +1707,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
     shifted_path.path.points, start_idx, distance_to_object);
 }
 
+void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const
+{
+  const auto & data = avoid_data_;
+
+  // do nothing if no shift line is approved.
+  if (path_shifter_.getShiftLines().empty()) {
+    return;
+  }
+
+  // do nothing if there is no avoidance target.
+  if (data.target_objects.empty()) {
+    return;
+  }
+
+  const auto [distance_to_accel_end_point, v_max] =
+    helper_->getDistanceToAccelEndPoint(shifted_path.path);
+  if (distance_to_accel_end_point < 1e-3) {
+    return;
+  }
+
+  const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
+  for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
+    const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+
+    // slow down speed is inserted only in front of the object.
+    const auto accel_distance = distance_to_accel_end_point - distance_from_ego;
+    if (accel_distance < 0.0) {
+      break;
+    }
+
+    const double v_target_square =
+      v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance;
+    if (v_target_square < 1e-3) {
+      break;
+    }
+
+    // target speed with nominal jerk limits.
+    const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square));
+    const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+    shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target);
+  }
+
+  slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
+    shifted_path.path.points, start_idx, distance_to_accel_end_point);
+}
+
 std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const
 {
   debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now();