diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 3dc5e7ee62a57..ae0a094e038a4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -58,6 +58,44 @@ double calc_dist_from_pose_to_terminal_end( * @return The required backward buffer distance in meters. */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); + +/** + * @brief Calculates the maximum preparation longitudinal distance for lane change. + * + * This function computes the maximum distance that the ego vehicle can travel during + * the preparation phase of a lane change. The distance is calculated as the product + * of the maximum lane change preparation duration and the maximum velocity of the ego vehicle. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - `lc_param_ptr->lane_change_prepare_duration`: The duration allowed for lane change + * preparation. + * - `bpp_param_ptr->max_vel`: The maximum velocity of the ego vehicle. + * + * @return The maximum preparation longitudinal distance in meters. + */ +double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr); + +/** + * @brief Calculates the distance from the ego vehicle to the start of the target lanes. + * + * This function computes the shortest distance from the current position of the ego vehicle + * to the start of the target lanes by measuring the arc length to the front points of + * the left and right boundaries of the target lane. If the target lanes are empty or other + * required data is unavailable, the function returns numeric_limits::max() preventing lane + * change being executed. + * + * @param common_data_ptr Shared pointer to a CommonData structure, which should include: + * - `route_handler_ptr`: Pointer to the route handler that manages the route. + * - Other necessary parameters for ego vehicle positioning. + * @param current_lanes The set of lanelets representing the current lanes of the ego vehicle. + * @param target_lanes The set of lanelets representing the target lanes for lane changing. + * + * @return The distance from the ego vehicle to the start of the target lanes in meters, + * or numeric_limits::max() if the target lanes are empty or data is unavailable. + */ +double calc_ego_dist_to_lanes_start( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 42b22f9b1631c..28efae2549d1e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -153,6 +153,8 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + const auto current_lanes = utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); @@ -162,7 +164,15 @@ bool NormalLaneChange::isLaneChangeRequired() const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !target_lanes.empty(); + if (target_lanes.empty()) { + return false; + } + + const auto ego_dist_to_target_start = + calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); + const auto maximum_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); + + return ego_dist_to_target_start <= maximum_prepare_length; } bool NormalLaneChange::isStoppedAtRedTrafficLight() const diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index ac205ceedb34b..2dc65a8b78045 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -54,4 +54,44 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; return std::max(min_back_dist, param_back_dist); } + +double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) +{ + const auto max_prepare_duration = common_data_ptr->lc_param_ptr->lane_change_prepare_duration; + const auto ego_max_speed = common_data_ptr->bpp_param_ptr->max_vel; + + return max_prepare_duration * ego_max_speed; +} + +double calc_ego_dist_to_lanes_start( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + + if (!route_handler_ptr || target_lanes.empty() || current_lanes.empty()) { + return std::numeric_limits::max(); + } + + const auto & target_bound = + common_data_ptr->direction == autoware::route_handler::Direction::RIGHT + ? target_lanes.front().leftBound() + : target_lanes.front().rightBound(); + + if (target_bound.empty()) { + return std::numeric_limits::max(); + } + + const auto path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + + if (path.points.empty()) { + return std::numeric_limits::max(); + } + + const auto target_front_pt = lanelet::utils::conversion::toGeomMsgPt(target_bound.front()); + const auto ego_position = common_data_ptr->get_ego_pose().position; + + return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation