refactor(lane_change): move lane change param (autowarefoundation#5807)

* refactor(lane_change): move lane change params Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): remove unnecessary param path Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
tier4 · Dec 8, 2023 · ca19947 · ca19947
1 parent dbfaabc
commit ca19947
Show file tree

Hide file tree

Showing 3 changed files with 0 additions and 80 deletions.
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp
@@ -15,7 +15,6 @@
 #ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_
 #define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_
 
-#include <interpolation/linear_interpolation.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <utility>
@@ -32,47 +31,6 @@ struct ModuleConfigParameters
   uint8_t max_module_size{0};
 };
 
-struct LateralAccelerationMap
-{
-  std::vector<double> base_vel;
-  std::vector<double> base_min_acc;
-  std::vector<double> base_max_acc;
-
-  void add(const double velocity, const double min_acc, const double max_acc)
-  {
-    if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) {
-      return;
-    }
-
-    size_t idx = 0;
-    for (size_t i = 0; i < base_vel.size(); ++i) {
-      if (velocity < base_vel.at(i)) {
-        break;
-      }
-      idx = i + 1;
-    }
-
-    base_vel.insert(base_vel.begin() + idx, velocity);
-    base_min_acc.insert(base_min_acc.begin() + idx, min_acc);
-    base_max_acc.insert(base_max_acc.begin() + idx, max_acc);
-  }
-
-  std::pair<double, double> find(const double velocity) const
-  {
-    if (!base_vel.empty() && velocity < base_vel.front()) {
-      return std::make_pair(base_min_acc.front(), base_max_acc.front());
-    }
-    if (!base_vel.empty() && velocity > base_vel.back()) {
-      return std::make_pair(base_min_acc.back(), base_max_acc.back());
-    }
-
-    const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity);
-    const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity);
-
-    return std::make_pair(min_acc, max_acc);
-  }
-};
-
 struct BehaviorPathPlannerParameters
 {
   bool verbose;
@@ -81,22 +39,13 @@ struct BehaviorPathPlannerParameters
 
   double backward_path_length;
   double forward_path_length;
-  double backward_length_buffer_for_end_of_lane;
-  double backward_length_buffer_for_blocking_object;
   double backward_length_buffer_for_end_of_pull_over;
   double backward_length_buffer_for_end_of_pull_out;
 
   // common parameters
   double min_acc;
   double max_acc;
 
-  // lane change parameters
-  double lane_changing_lateral_jerk{0.5};
-  double minimum_lane_changing_velocity{5.6};
-  double lane_change_prepare_duration{4.0};
-  double lane_change_finish_judge_buffer{3.0};
-  LateralAccelerationMap lane_change_lat_acc_map;
-
   double minimum_pull_over_length;
   double minimum_pull_out_length;
   double drivable_area_resolution;

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp
@@ -333,10 +333,6 @@ lanelet::ConstLanelets calcLaneAroundPose(
 
 bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 
-double calcMinimumLaneChangeLength(
-  const BehaviorPathPlannerParameters & common_param, const std::vector<double> & shift_intervals,
-  const double backward_buffer, const double length_to_intersection = 0.0);
-
 lanelet::ConstLanelets getLaneletsFromPath(
   const PathWithLaneId & path, const std::shared_ptr<route_handler::RouteHandler> & route_handler);
 

diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp
@@ -1515,31 +1515,6 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre
   return true;
 }
 
-double calcMinimumLaneChangeLength(
-  const BehaviorPathPlannerParameters & common_param, const std::vector<double> & shift_intervals,
-  const double backward_buffer, const double length_to_intersection)
-{
-  if (shift_intervals.empty()) {
-    return 0.0;
-  }
-
-  const double & vel = common_param.minimum_lane_changing_velocity;
-  const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel);
-  const double & max_lateral_acc = lat_acc.second;
-  const double & lateral_jerk = common_param.lane_changing_lateral_jerk;
-  const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer;
-
-  double accumulated_length = length_to_intersection;
-  for (const auto & shift_interval : shift_intervals) {
-    const double t =
-      PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc);
-    accumulated_length += vel * t + finish_judge_buffer;
-  }
-  accumulated_length += backward_buffer * (shift_intervals.size() - 1.0);
-
-  return accumulated_length;
-}
-
 lanelet::ConstLanelets getLaneletsFromPath(
   const PathWithLaneId & path, const std::shared_ptr<route_handler::RouteHandler> & route_handler)
 {