From a2adc344d2ce26f381e2460284ae335c08f1a280 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 25 Oct 2023 11:39:12 +0900 Subject: [PATCH 1/4] fix(lane_change): fix terminal stop distance (#5392) * fix(lane_change): fix terminal stop distance Signed-off-by: kosuke55 * make current lanes include path front id Signed-off-by: kosuke55 * fixup! make current lanes include path front id --------- Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utils/utils.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index ed2979bd1d5a1..a794b229fdc6c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2873,9 +2873,24 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); - - return route_handler->getLaneletSequence( + auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); + + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'. + const auto front_lane_ids = path.points.front().lane_ids; + auto have_front_lanes = [front_lane_ids](const auto & lanes) { + return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { + return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) != + front_lane_ids.end(); + }); + }; + while (!have_front_lanes(current_lanes)) { + const auto extended_lanes = extendPrevLane(route_handler, current_lanes); + if (extended_lanes.size() == current_lanes.size()) break; + current_lanes = extended_lanes; + } + + return current_lanes; } lanelet::ConstLanelets extendNextLane( From 82a9c13c88922a28e3d3e3d88579eb911bd89702 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 25 Oct 2023 17:19:21 +0900 Subject: [PATCH 2/4] fix(behavior_path_planner): add guard to extend lane Signed-off-by: kosuke55 --- planning/behavior_path_planner/src/utils/utils.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index a794b229fdc6c..44ff2eaaba5a9 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2896,6 +2896,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelets extendNextLane( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add next lane @@ -2917,6 +2919,8 @@ lanelet::ConstLanelets extendNextLane( lanelet::ConstLanelets extendPrevLane( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add previous lane From 9ae613df7b6d12778b51e0dccd3d6ca6f7d53580 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 26 Oct 2023 11:26:00 +0900 Subject: [PATCH 3/4] fix(behavior_path_planner): fix lane extension in getCurrentLanesFromPath Signed-off-by: kosuke55 --- .../behavior_path_planner/utils/utils.hpp | 6 ++- .../behavior_path_planner/src/utils/utils.cpp | 40 ++++++++++++++----- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index be227f4f7e93e..a96c8b320f837 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -363,10 +363,12 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 44ff2eaaba5a9..5b57b2a6e4809 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2876,7 +2876,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); - // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'. + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids' + // if the extended prior lanes is in same lane sequence with current lanes const auto front_lane_ids = path.points.front().lane_ids; auto have_front_lanes = [front_lane_ids](const auto & lanes) { return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { @@ -2884,17 +2885,23 @@ lanelet::ConstLanelets getCurrentLanesFromPath( front_lane_ids.end(); }); }; - while (!have_front_lanes(current_lanes)) { - const auto extended_lanes = extendPrevLane(route_handler, current_lanes); - if (extended_lanes.size() == current_lanes.size()) break; - current_lanes = extended_lanes; + auto extended_lanes = current_lanes; + while (rclcpp::ok()) { + const size_t pre_extension_size = extended_lanes.size(); // Get existing size before extension + extended_lanes = extendPrevLane(route_handler, extended_lanes); + if (extended_lanes.size() == pre_extension_size) break; + if (have_front_lanes(extended_lanes)) { + current_lanes = extended_lanes; + break; + } } return current_lanes; } lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { if (lanes.empty()) return lanes; @@ -2903,21 +2910,27 @@ lanelet::ConstLanelets extendNextLane( // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { + boost::optional target_next_lane; + if (!only_in_route) { + target_next_lane = next_lanes.front(); + } // use the next lane in route if it exists - auto target_next_lane = next_lanes.front(); for (const auto & next_lane : next_lanes) { if (route_handler->isRouteLanelet(next_lane)) { target_next_lane = next_lane; } } - extended_lanes.push_back(target_next_lane); + if (target_next_lane) { + extended_lanes.push_back(*target_next_lane); + } } return extended_lanes; } lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { if (lanes.empty()) return lanes; @@ -2926,14 +2939,19 @@ lanelet::ConstLanelets extendPrevLane( // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { + boost::optional target_prev_lane; + if (!only_in_route) { + target_prev_lane = prev_lanes.front(); + } // use the previous lane in route if it exists - auto target_prev_lane = prev_lanes.front(); for (const auto & prev_lane : prev_lanes) { if (route_handler->isRouteLanelet(prev_lane)) { target_prev_lane = prev_lane; } } - extended_lanes.insert(extended_lanes.begin(), target_prev_lane); + if (target_prev_lane) { + extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); + } } return extended_lanes; } From 3f7ceaaf077e7a8577742f2a20e2dfeb01080aa8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Thu, 26 Oct 2023 14:36:31 +0900 Subject: [PATCH 4/4] fix Signed-off-by: kosuke55 --- planning/behavior_path_planner/src/utils/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 5b57b2a6e4809..9c298b13b3e76 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2888,7 +2888,7 @@ lanelet::ConstLanelets getCurrentLanesFromPath( auto extended_lanes = current_lanes; while (rclcpp::ok()) { const size_t pre_extension_size = extended_lanes.size(); // Get existing size before extension - extended_lanes = extendPrevLane(route_handler, extended_lanes); + extended_lanes = extendPrevLane(route_handler, extended_lanes, true); if (extended_lanes.size() == pre_extension_size) break; if (have_front_lanes(extended_lanes)) { current_lanes = extended_lanes;