diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 5dcd132f16377..1386c60d29aea 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -383,6 +383,22 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); +/** + * @brief Retrieves the preceding lanes for the target lanes while removing overlapping and + * disconnected lanes. + * + * This function identifies all lanes that precede the target lanes based on the ego vehicle's + * current position and a specified backward search length. The resulting preceding lanes are + * filtered to remove lanes that overlap with the current lanes or are not connected to the route. + * + * @param common_data_ptr Shared pointer to commonly used data in lane change module, which contains + * route handler information, lane details, ego vehicle pose, and behavior parameters. + * + * @return A vector of preceding lanelet groups, with each group containing only the connected and + * non-overlapping preceding lanes. + */ +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr); + /** * @brief Determines if the object's predicted path overlaps with the given lane polygon. * 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 2770831dbc485..5b8b8966f965b 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 @@ -112,9 +112,8 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(target_lanes.back()); - common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), - common_data_ptr_->lc_param_ptr->backward_lane_length); + common_data_ptr_->lanes_ptr->preceding_target = + utils::lane_change::get_preceding_lanes(common_data_ptr_); lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 4d46f8270fac3..13a119848a87a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -34,9 +34,12 @@ #include #include #include +#include #include +#include +#include #include -#include +#include #include #include @@ -56,6 +59,7 @@ #include #include #include +#include #include namespace autoware::behavior_path_planner::utils::lane_change @@ -1246,6 +1250,43 @@ bool filter_target_lane_objects( return false; } +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & ego_pose = common_data_ptr->get_ego_pose(); + const auto backward_lane_length = common_data_ptr->lc_param_ptr->backward_lane_length; + + const auto preceding_lanes_list = + utils::getPrecedingLanelets(*route_handler_ptr, target_lanes, ego_pose, backward_lane_length); + + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + std::unordered_set current_lanes_id; + for (const auto & lane : current_lanes) { + current_lanes_id.insert(lane.id()); + } + const auto is_overlapping = [&](const lanelet::ConstLanelet & lane) { + return current_lanes_id.find(lane.id()) != current_lanes_id.end(); + }; + + std::vector non_overlapping_lanes_vec; + for (const auto & lanes : preceding_lanes_list) { + auto lanes_reversed = lanes | ranges::views::reverse; + auto overlapped_itr = ranges::find_if(lanes_reversed, is_overlapping); + + if (overlapped_itr == lanes_reversed.begin()) { + continue; + } + + // Lanes are not reversed by default. Avoid returning reversed lanes to prevent undefined + // behavior. + lanelet::ConstLanelets non_overlapping_lanes(overlapped_itr.base(), lanes.end()); + non_overlapping_lanes_vec.push_back(non_overlapping_lanes); + } + + return non_overlapping_lanes_vec; +} + bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) {