diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 41420430d69d5..2d427213141b4 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -296,7 +296,7 @@ double calcPhaseLength( LanesPolygon createLanesPolygon( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const lanelet::ConstLanelets & target_backward_lanes); + const std::vector & target_backward_lanes); } // namespace behavior_path_planner::utils::lane_change namespace behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 67d8e1968c7e4..7a1419547e712 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1022,14 +1022,45 @@ void NormalLaneChange::filterObjectsByLanelets( // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( - *route_handler, target_lanes, current_pose, backward_length); + const auto target_backward_lanes = + [&]() -> std::vector { + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + return route_handler->getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); + }(); { lane_change_debug_.current_lanes = current_lanes; lane_change_debug_.target_lanes = target_lanes; - lane_change_debug_.target_backward_lanes = target_backward_lanes; + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&target_backward_lanes]() { + size_t sum{0}; + for (const auto & lanes : target_backward_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : target_backward_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); } + + lane_change_debug_.target_backward_lanes = backward_lanes; + } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, lane_change_parameters_->lane_expansion_right_offset); diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 214e20c2fb645..222bd4e01deb7 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1227,7 +1227,7 @@ double calcPhaseLength( LanesPolygon createLanesPolygon( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const lanelet::ConstLanelets & target_backward_lanes) + const std::vector & target_backward_lanes) { LanesPolygon lanes_polygon; @@ -1237,19 +1237,8 @@ LanesPolygon createLanesPolygon( utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); for (const auto & target_backward_lane : target_backward_lanes) { - // Check to see is target_backward_lane is in current_lanes - // Without this check, current lane object might be treated as target lane object - const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { - return current_lane.id() == target_backward_lane.id(); - }; - - if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { - continue; - } - - lanelet::ConstLanelets lanelet{target_backward_lane}; auto lane_polygon = - utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(target_backward_lane, 0.0, std::numeric_limits::max()); if (lane_polygon) { lanes_polygon.target_backward.push_back(*lane_polygon);