feat(behavior_path_planner): remove calcTotalLaneChangeLength function (

autowarefoundation#3483) Signed-off-by: yutaka <[email protected]>
soblin · Apr 20, 2023 · ff37c36 · ff37c36
1 parent 85acbea
commit ff37c36
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 14 deletions.
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -182,7 +182,10 @@ class LaneChangeBase
   bool isNearEndOfLane() const
   {
     const auto & current_pose = getEgoPose();
-    const double threshold = utils::calcTotalLaneChangeLength(planner_data_->parameters);
+    const auto shift_intervals = planner_data_->route_handler->getLateralIntervalsToPreferredLane(
+      status_.current_lanes.back());
+    const double threshold =
+      utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals);
 
     return (std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) -
             threshold) < planner_data_->parameters.backward_length_buffer_for_end_of_lane;

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -336,9 +336,6 @@ boost::optional<std::pair<Pose, Polygon2d>> getEgoExpectedPoseAndConvertToPolygo
 
 bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 
-double calcTotalLaneChangeLength(
-  const BehaviorPathPlannerParameters & common_param, const bool include_buffer = true);
-
 double calcLaneChangingTime(
   const double lane_changing_velocity, const double shift_length,
   const BehaviorPathPlannerParameters & common_parameter);

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp
@@ -727,7 +727,10 @@ bool AvoidanceByLCModule::isValidPath(const PathWithLaneId & path) const
 bool AvoidanceByLCModule::isNearEndOfLane() const
 {
   const auto & current_pose = getEgoPose();
-  const double threshold = utils::calcTotalLaneChangeLength(planner_data_->parameters);
+  const auto shift_intervals =
+    planner_data_->route_handler->getLateralIntervalsToPreferredLane(status_.current_lanes.back());
+  const double threshold =
+    utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals);
 
   return std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) <
          threshold;

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1973,15 +1973,6 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre
   return true;
 }
 
-double calcTotalLaneChangeLength(
-  const BehaviorPathPlannerParameters & common_param, const bool include_buffer)
-{
-  const double minimum_lane_change_distance =
-    common_param.minimum_prepare_length + common_param.minimum_lane_changing_length;
-  const double end_of_lane_buffer = common_param.backward_length_buffer_for_end_of_lane;
-  return minimum_lane_change_distance + end_of_lane_buffer * static_cast<double>(include_buffer);
-}
-
 double calcLaneChangingTime(
   const double lane_changing_velocity, const double shift_length,
   const BehaviorPathPlannerParameters & common_parameter)