From 63cd27621e952fcb838ae4a284afe65a767a37b8 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Sat, 19 Nov 2022 11:24:03 +0900 Subject: [PATCH] feat(behavior_path_planner): fix lane change drivable lanes (#2324) * fix(behavior_path_planner): delete unnecessary codes Signed-off-by: yutaka * feat(behavior_path_planner): fix lane change drivable lanes Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: yoshiri --- .../src/scene_module/lane_change/util.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 5beaa0a39bb9a..f3c1a0038df23 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -631,6 +631,7 @@ std::vector generateDrivableLanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) { + size_t current_lc_idx = 0; std::vector drivable_lanes(current_lanes.size()); for (size_t i = 0; i < current_lanes.size(); ++i) { const auto & current_lane = current_lanes.at(i); @@ -643,19 +644,30 @@ std::vector generateDrivableLanes( continue; } - for (const auto & lc_lane : lane_change_lanes) { + for (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) { + const auto & lc_lane = lane_change_lanes.at(lc_idx); if (left_lane && lc_lane.id() == left_lane->id()) { drivable_lanes.at(i).left_lane = lc_lane; + current_lc_idx = lc_idx; break; } if (right_lane && lc_lane.id() == right_lane->id()) { drivable_lanes.at(i).right_lane = lc_lane; + current_lc_idx = lc_idx; break; } } } + for (size_t i = current_lc_idx + 1; i < lane_change_lanes.size(); ++i) { + const auto & lc_lane = lane_change_lanes.at(i); + DrivableLanes drivable_lane; + drivable_lane.left_lane = lc_lane; + drivable_lane.right_lane = lc_lane; + drivable_lanes.push_back(drivable_lane); + } + return drivable_lanes; }