diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 91d777204e28c..6959fcb91616f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_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 index 3b53b16993233..3bcd01d058dba 100644 --- 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> 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 index 48c7f3477f21c..ac824c8492b65 100644 --- 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 index 172ce8f2b25b1..2903a28001b4e 100644 --- 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(include_buffer); -} - double calcLaneChangingTime( const double lane_changing_velocity, const double shift_length, const BehaviorPathPlannerParameters & common_parameter)