From d87301c1e5273dcf7adbb60d3a6d34359cbce93f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 22 Aug 2024 13:23:52 +0900 Subject: [PATCH] fix(lane_change): modify lane change requested condition (#8510) * modify lane change requested condition Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> * style(pre-commit): autofix * fix docstring Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../utils/calculation.hpp | 38 ++++++++++++++++++ .../src/scene.cpp | 12 +++++- .../src/utils/calculation.cpp | 40 +++++++++++++++++++ 3 files changed, 89 insertions(+), 1 deletion(-) 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 6c48445b47567..66c454f795ebb 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 @@ -75,6 +75,44 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); double calc_dist_to_last_fit_width( const lanelet::ConstLanelets lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); + +/** + * @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 e3faf8f4293c7..c6d05f90fe138 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 @@ -152,6 +152,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_); @@ -161,7 +163,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 8f04395be572a..9585449b8e0e2 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 @@ -101,4 +101,44 @@ double calc_dist_to_last_fit_width( return std::max(distance - (bpp_param.base_link2front + margin), 0.0); } + +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