From 964095c63d8474f70307685e87e15f3d677098a2 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 16 Jun 2023 08:50:43 +0900 Subject: [PATCH] refactor(utils): merge similar functions Signed-off-by: satoshi-ota --- .../utils/avoidance/helper.hpp | 23 ++++--------------- .../avoidance/avoidance_module.cpp | 6 ++--- 2 files changed, 8 insertions(+), 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 index 2656f1bd48d68..76f8de5bd1244 100644 --- 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::max(); - } - - double getMildDecelDistance(const double target_velocity) const + double getFeasibleDecelDistance( + const double target_velocity, const bool use_max_value = 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_max_value ? p->max_deceleration : p->nominal_deceleration; + const auto & j_lim = use_max_value ? 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 index 8670f0b2f4bc5..b04cf59f363c6 100644 --- 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_); }