refactor(utils): merge similar functions

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jun 15, 2023 · 23940ea · 23940ea
1 parent 65b0b6d
commit 23940ea
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 21 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp
@@ -198,26 +198,13 @@ class AvoidanceHelper
     return *itr;
   }
 
-  double getFeasibleDecelDistance(const double target_velocity) const
-  {
-    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
-    const auto & a_lim = parameters_->max_deceleration;
-    const auto & j_lim = parameters_->max_jerk;
-    const auto ret = calcDecelDistWithJerkAndAccConstraints(
-      getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
-
-    if (!!ret) {
-      return ret.get();
-    }
-
-    return std::numeric_limits<double>::max();
-  }
-
-  double getMildDecelDistance(const double target_velocity) const
+  double getFeasibleDecelDistance(
+    const double target_velocity, const bool use_hard_constraints = true) const
   {
+    const auto & p = parameters_;
     const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
-    const auto & a_lim = parameters_->nominal_deceleration;
-    const auto & j_lim = parameters_->nominal_jerk;
+    const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration;
+    const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk;
     const auto ret = calcDecelDistWithJerkAndAccConstraints(
       getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -3320,7 +3320,7 @@ void AvoidanceModule::insertWaitPoint(
 
   // If the object cannot be stopped for, calculate a "mild" deceleration distance
   // and insert a deceleration point at that distance
-  const auto stop_distance = helper_.getMildDecelDistance(0.0);
+  const auto stop_distance = helper_.getFeasibleDecelDistance(0.0, false);
   utils::avoidance::insertDecelPoint(
     getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_);
 }
@@ -3363,7 +3363,7 @@ void AvoidanceModule::insertStopPoint(
   }
 
   // Otherwise, consider deceleration constraints before inserting deceleration point
-  const auto decel_distance = helper_.getMildDecelDistance(0.0);
+  const auto decel_distance = helper_.getFeasibleDecelDistance(0.0, false);
   if (stop_distance < decel_distance) {
     return;
   }
@@ -3386,7 +3386,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const
     return;
   }
 
-  const auto decel_distance = helper_.getMildDecelDistance(p->yield_velocity);
+  const auto decel_distance = helper_.getFeasibleDecelDistance(p->yield_velocity, false);
   utils::avoidance::insertDecelPoint(
     getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_);
 }