From 3fcee3309d5cd055afc687ad9bd31bff58d0e5eb Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Mon, 7 Mar 2022 15:57:37 +0900 Subject: [PATCH 1/9] refactor(avoidance_module): change implementation to lambda In the generateExtendedDrivableArea function, the implementation to extend drivable areas with reference to object is refactor to lambda. This will allow the function to be used for some future planned updates. The refactor doesn't affect the behavior of the avoidance_module. Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 40 ++++++++++++------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 9bb114b2b5785..26c70cf64214b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1674,35 +1674,47 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c { // 0. Extend to right/left of objects - for (const auto & obstacle : avoidance_data_.objects) { - auto object_lanelet = obstacle.overhang_lanelet; - if (isOnRight(obstacle)) { - auto lanelet_at_left = route_handler->getLeftLanelet(object_lanelet); + const auto searchLeftLaneletsAndAppendToDrivableAreas = + [&route_handler]( + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended) noexcept { + auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet); while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_to_be_extended.push_back(lanelet_at_left.get()); lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } - if (lanelet_at_left) { - auto lanelet_at_right = - planner_data_->route_handler->getRightLanelet(lanelet_at_left.get()); + + if (lanelet_at_left) { // means lanelets in the opposite direction exist + auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left.get()); while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_to_be_extended.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } } - } else { - auto lanelet_at_right = route_handler->getRightLanelet(object_lanelet); + }; + + const auto searchRightLaneletsAndAppendToDrivableAreas = + [&route_handler]( + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended) noexcept { + auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet); while (lanelet_at_right) { - extended_lanelets.push_back(lanelet_at_right.get()); + lanelet_to_be_extended.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } - if (lanelet_at_right) { + if (lanelet_at_right) { // means lanelets in the opposite direction exist auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); while (lanelet_at_left) { - extended_lanelets.push_back(lanelet_at_left.get()); + lanelet_to_be_extended.push_back(lanelet_at_left.get()); lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } } + }; + + for (const auto & obstacle : avoidance_data_.objects) { + auto object_lanelet = obstacle.overhang_lanelet; + if (isOnRight(obstacle)) { + searchLeftLaneletsAndAppendToDrivableAreas(object_lanelet, extended_lanelets); + } else { + searchRightLaneletsAndAppendToDrivableAreas(object_lanelet, extended_lanelets); } } } From c0f7bb12f2f540e6238d28c2d1f0e525d7a6d3b1 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Mon, 7 Mar 2022 17:23:09 +0900 Subject: [PATCH 2/9] refactor: add default flag to extend_to_opposite side By default, the parameter is true, therefore it doesn't affect the original behavior. Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/avoidance/avoidance_module.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 26c70cf64214b..7d1ddbf9cf26d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1676,14 +1676,16 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c // 0. Extend to right/left of objects const auto searchLeftLaneletsAndAppendToDrivableAreas = [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended) noexcept { + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, + bool extend_to_opposite_lane = true) noexcept { auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet); while (lanelet_at_left) { lanelet_to_be_extended.push_back(lanelet_at_left.get()); lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } - if (lanelet_at_left) { // means lanelets in the opposite direction exist + if (lanelet_at_left && extend_to_opposite_lane) { // means lanelets in the opposite + // direction exist auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left.get()); while (lanelet_at_right) { lanelet_to_be_extended.push_back(lanelet_at_right.get()); @@ -1694,13 +1696,15 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c const auto searchRightLaneletsAndAppendToDrivableAreas = [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended) noexcept { + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, + bool extend_to_opposite_lane = true) noexcept { auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet); while (lanelet_at_right) { lanelet_to_be_extended.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } - if (lanelet_at_right) { // means lanelets in the opposite direction exist + if (lanelet_at_right && extend_to_opposite_lane) { // means lanelets in the opposite + // direction exist auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); while (lanelet_at_left) { lanelet_to_be_extended.push_back(lanelet_at_left.get()); From 4fe68c9cc618da64e63467797d8cc1932620ea12 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 7 Mar 2022 08:26:03 +0000 Subject: [PATCH 3/9] ci(pre-commit): autofix --- .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 7d1ddbf9cf26d..fbfe84e041ccd 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1685,7 +1685,7 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c } if (lanelet_at_left && extend_to_opposite_lane) { // means lanelets in the opposite - // direction exist + // direction exist auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left.get()); while (lanelet_at_right) { lanelet_to_be_extended.push_back(lanelet_at_right.get()); @@ -1704,7 +1704,7 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } if (lanelet_at_right && extend_to_opposite_lane) { // means lanelets in the opposite - // direction exist + // direction exist auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); while (lanelet_at_left) { lanelet_to_be_extended.push_back(lanelet_at_left.get()); From 84132a64af3f70811bc354c7abbaf624fc4f79a7 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Tue, 8 Mar 2022 23:45:05 +0900 Subject: [PATCH 4/9] fix: rework function for better extension Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 75 ++++++++++--------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index fbfe84e041ccd..fa00b808e19a8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1672,46 +1672,53 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c const auto & route_handler = planner_data_->route_handler; lanelet::ConstLanelets extended_lanelets = avoidance_data_.current_lanelets; - { - // 0. Extend to right/left of objects - const auto searchLeftLaneletsAndAppendToDrivableAreas = - [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, - bool extend_to_opposite_lane = true) noexcept { - auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - } - - if (lanelet_at_left && extend_to_opposite_lane) { // means lanelets in the opposite - // direction exist - auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left.get()); - while (lanelet_at_right) { - lanelet_to_be_extended.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); - } - } - }; + const auto searchLeftLaneletsAndAppendToDrivableAreas = + [&route_handler]( + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, + bool extend_to_opposite_lane = true) noexcept { + auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet); + auto lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(current_lanelet); + while (lanelet_at_left) { + lanelet_to_be_extended.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); + lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(lanelet_at_left.get()); + } - const auto searchRightLaneletsAndAppendToDrivableAreas = - [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, - bool extend_to_opposite_lane = true) noexcept { - auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet); + if (!lanelet_at_left_opposite.empty() && extend_to_opposite_lane) { + auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left_opposite.front()); while (lanelet_at_right) { lanelet_to_be_extended.push_back(lanelet_at_right.get()); lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); } - if (lanelet_at_right && extend_to_opposite_lane) { // means lanelets in the opposite - // direction exist - auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right.get()); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - } + } + }; + + const auto searchRightLaneletsAndAppendToDrivableAreas = + [&route_handler]( + const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, + bool extend_to_opposite_lane = true) noexcept { + auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet); + auto lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(current_lanelet); + while (lanelet_at_right) { + lanelet_to_be_extended.push_back(lanelet_at_right.get()); + lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); + lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(lanelet_at_right.get()); + } + + if (!lanelet_at_right_opposite.empty() && extend_to_opposite_lane) { // means lanelets in + // the opposite + // direction exist + lanelet_to_be_extended.push_back(lanelet_at_right_opposite.front()); + auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + lanelet_to_be_extended.push_back(lanelet_at_left.get()); + lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); } - }; + } + }; + + { + // 0. Extend to right/left of objects for (const auto & obstacle : avoidance_data_.objects) { auto object_lanelet = obstacle.overhang_lanelet; From ea262a4ea8fe10061fce6edecc0d868bc99c9e10 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Wed, 9 Mar 2022 13:18:35 +0900 Subject: [PATCH 5/9] refactor(route handler, avoidance_module): move the implementation The implementation is made into function and moved into route handler Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 58 ++++-------------- .../include/route_handler/route_handler.hpp | 4 ++ planning/route_handler/src/route_handler.cpp | 59 +++++++++++++++++++ 3 files changed, 73 insertions(+), 48 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index fa00b808e19a8..bbe7eb54bfeac 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1672,61 +1672,23 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c const auto & route_handler = planner_data_->route_handler; lanelet::ConstLanelets extended_lanelets = avoidance_data_.current_lanelets; - const auto searchLeftLaneletsAndAppendToDrivableAreas = - [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, - bool extend_to_opposite_lane = true) noexcept { - auto lanelet_at_left = route_handler->getLeftLanelet(current_lanelet); - auto lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(current_lanelet); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - lanelet_at_left_opposite = route_handler->getLeftOppositeLanelets(lanelet_at_left.get()); - } - - if (!lanelet_at_left_opposite.empty() && extend_to_opposite_lane) { - auto lanelet_at_right = route_handler->getRightLanelet(lanelet_at_left_opposite.front()); - while (lanelet_at_right) { - lanelet_to_be_extended.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); - } - } - }; - - const auto searchRightLaneletsAndAppendToDrivableAreas = - [&route_handler]( - const lanelet::ConstLanelet & current_lanelet, auto & lanelet_to_be_extended, - bool extend_to_opposite_lane = true) noexcept { - auto lanelet_at_right = route_handler->getRightLanelet(current_lanelet); - auto lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(current_lanelet); - while (lanelet_at_right) { - lanelet_to_be_extended.push_back(lanelet_at_right.get()); - lanelet_at_right = route_handler->getRightLanelet(lanelet_at_right.get()); - lanelet_at_right_opposite = route_handler->getRightOppositeLanelets(lanelet_at_right.get()); - } - - if (!lanelet_at_right_opposite.empty() && extend_to_opposite_lane) { // means lanelets in - // the opposite - // direction exist - lanelet_to_be_extended.push_back(lanelet_at_right_opposite.front()); - auto lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_right_opposite.front()); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = route_handler->getLeftLanelet(lanelet_at_left.get()); - } - } - }; - { // 0. Extend to right/left of objects - for (const auto & obstacle : avoidance_data_.objects) { + lanelet::ConstLanelets search_lanelets; auto object_lanelet = obstacle.overhang_lanelet; + constexpr bool get_right = true; + constexpr bool get_left = true; + const bool include_opposite = parameters_.enable_avoidance_over_opposite_direction; if (isOnRight(obstacle)) { - searchLeftLaneletsAndAppendToDrivableAreas(object_lanelet, extended_lanelets); + search_lanelets = route_handler->getAllSharedLineStringLanelets( + object_lanelet, !get_right, get_left, include_opposite); } else { - searchRightLaneletsAndAppendToDrivableAreas(object_lanelet, extended_lanelets); + search_lanelets = route_handler->getAllSharedLineStringLanelets( + object_lanelet, get_right, !get_left, include_opposite); } + extended_lanelets.insert( + extended_lanelets.end(), search_lanelets.begin(), search_lanelets.end()); } } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7cf38a40d0ee2..b01558882be51 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -138,6 +138,10 @@ class RouteHandler */ lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, + bool is_opposite = true) const noexcept; + /** * @brief Searches the furthest linestring to the right side of the lanelet * Only lanelet with same direction is considered diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ac48a20ea0209..f7f254dc18a4c 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -911,6 +911,65 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets( return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); } +lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, + bool is_opposite) const noexcept +{ + const auto searchAllLeftLanelets = [this, &is_opposite]( + const lanelet::ConstLanelet & current_lanelet, + auto & lanelet_to_be_extended) noexcept { + auto lanelet_at_left = getLeftLanelet(current_lanelet); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(current_lanelet); + while (lanelet_at_left) { + lanelet_to_be_extended.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get()); + } + + if (!lanelet_at_left_opposite.empty() && is_opposite) { + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + lanelet_to_be_extended.push_back(lanelet_at_right.get()); + lanelet_at_right = getRightLanelet(lanelet_at_right.get()); + } + } + }; + + const auto searchAllRightLanelets = [this, &is_opposite]( + const lanelet::ConstLanelet & current_lanelet, + auto & lanelet_to_be_extended) noexcept { + auto lanelet_at_right = getRightLanelet(current_lanelet); + auto lanelet_at_right_opposite = getRightOppositeLanelets(current_lanelet); + while (lanelet_at_right) { + lanelet_to_be_extended.push_back(lanelet_at_right.get()); + lanelet_at_right = getRightLanelet(lanelet_at_right.get()); + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get()); + } + + if (!lanelet_at_right_opposite.empty() && is_opposite) { // means lanelets in + // the opposite + // direction exist + lanelet_to_be_extended.push_back(lanelet_at_right_opposite.front()); + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + lanelet_to_be_extended.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); + } + } + }; + + lanelet::ConstLanelets shared_linestring_lanelet; + shared_linestring_lanelet.push_back(current_lane); + if (is_right) { + searchAllRightLanelets(current_lane, shared_linestring_lanelet); + } + if (is_left) { + searchAllLeftLanelets(current_lane, shared_linestring_lanelet); + } + + return shared_linestring_lanelet; +} + lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const { return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); From d2162c5d3ea6d849a782a7cac23ac04597f9aaaf Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Wed, 9 Mar 2022 22:03:39 +0900 Subject: [PATCH 6/9] Refactor: convert lambdas to function Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/route_handler/route_handler.hpp | 6 ++ planning/route_handler/src/route_handler.cpp | 96 ++++++++++--------- 2 files changed, 58 insertions(+), 44 deletions(-) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index b01558882be51..082b327108913 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -138,6 +138,12 @@ class RouteHandler */ lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + + lanelet::ConstLanelets getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + lanelet::ConstLanelets getAllSharedLineStringLanelets( const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, bool is_opposite = true) const noexcept; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f7f254dc18a4c..c4160d5f0b0f2 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -911,63 +911,71 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets( return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); } -lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( - const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, - bool is_opposite) const noexcept +lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept { - const auto searchAllLeftLanelets = [this, &is_opposite]( - const lanelet::ConstLanelet & current_lanelet, - auto & lanelet_to_be_extended) noexcept { - auto lanelet_at_left = getLeftLanelet(current_lanelet); - auto lanelet_at_left_opposite = getLeftOppositeLanelets(current_lanelet); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); - lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get()); - } - - if (!lanelet_at_left_opposite.empty() && is_opposite) { - auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); - while (lanelet_at_right) { - lanelet_to_be_extended.push_back(lanelet_at_right.get()); - lanelet_at_right = getRightLanelet(lanelet_at_right.get()); - } - } - }; + lanelet::ConstLanelets shared; + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + shared.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get()); + } - const auto searchAllRightLanelets = [this, &is_opposite]( - const lanelet::ConstLanelet & current_lanelet, - auto & lanelet_to_be_extended) noexcept { - auto lanelet_at_right = getRightLanelet(current_lanelet); - auto lanelet_at_right_opposite = getRightOppositeLanelets(current_lanelet); + if (!lanelet_at_left_opposite.empty() && include_opposite) { + shared.push_back(lanelet_at_left_opposite.front()); + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); while (lanelet_at_right) { - lanelet_to_be_extended.push_back(lanelet_at_right.get()); + shared.push_back(lanelet_at_right.get()); lanelet_at_right = getRightLanelet(lanelet_at_right.get()); - lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get()); } + } + return shared; +} - if (!lanelet_at_right_opposite.empty() && is_opposite) { // means lanelets in - // the opposite - // direction exist - lanelet_to_be_extended.push_back(lanelet_at_right_opposite.front()); - auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); - while (lanelet_at_left) { - lanelet_to_be_extended.push_back(lanelet_at_left.get()); - lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); - } +lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( + const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept +{ + lanelet::ConstLanelets shared; + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + shared.push_back(lanelet_at_right.get()); + lanelet_at_right = getRightLanelet(lanelet_at_right.get()); + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get()); + } + + if (!lanelet_at_right_opposite.empty() && include_opposite) { + shared.push_back(lanelet_at_right_opposite.front()); + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + shared.push_back(lanelet_at_left.get()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); } - }; + } + return shared; +} + +lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( + const lanelet::ConstLanelet & current_lane, bool is_right, bool is_left, + bool is_opposite) const noexcept +{ + lanelet::ConstLanelets shared{current_lane}; - lanelet::ConstLanelets shared_linestring_lanelet; - shared_linestring_lanelet.push_back(current_lane); if (is_right) { - searchAllRightLanelets(current_lane, shared_linestring_lanelet); + const lanelet::ConstLanelets all_right_lanelets = + getAllRightSharedLinestringLanelets(current_lane, is_opposite); + shared.insert(shared.end(), all_right_lanelets.begin(), all_right_lanelets.end()); } + if (is_left) { - searchAllLeftLanelets(current_lane, shared_linestring_lanelet); + const lanelet::ConstLanelets all_left_lanelets = + getAllLeftSharedLinestringLanelets(current_lane, is_opposite); + shared.insert(shared.end(), all_left_lanelets.begin(), all_left_lanelets.end()); } - return shared_linestring_lanelet; + return shared; } lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const From 65313d439c74f56d2b40df843dd3df9d4c3b93e1 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Thu, 10 Mar 2022 10:37:20 +0900 Subject: [PATCH 7/9] chore: add comments for comments for documentation in the header file Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/route_handler/route_handler.hpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 082b327108913..7db4f225aa813 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -138,12 +138,32 @@ class RouteHandler */ lanelet::Lanelets getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const; + /** + * @brief Searches and return all lanelet on the left that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ lanelet::ConstLanelets getAllLeftSharedLinestringLanelets( const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + /** + * @brief Searches and return all lanelet on the right that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ lanelet::ConstLanelets getAllRightSharedLinestringLanelets( const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept; + /** + * @brief Searches and return all lanelet (left and right) that shares same linestring + * @param the lanelet of interest + * @param (optional) flag to search only right side + * @param (optional) flag to search only left side + * @param (optional) flag to include the lane with opposite direction + * @return vector of lanelet that is connected via share linestring + */ lanelet::ConstLanelets getAllSharedLineStringLanelets( const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, bool is_opposite = true) const noexcept; From 7b2433f2ed79d04c6c32e85ad44789d16c19a402 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Fri, 11 Mar 2022 10:47:49 +0900 Subject: [PATCH 8/9] Refactor: local variable rename Signed-off-by: Muhammad Zulfaqar Azmi --- planning/route_handler/src/route_handler.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index c4160d5f0b0f2..2b47a6857b649 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -914,47 +914,47 @@ lanelet::Lanelets RouteHandler::getRightOppositeLanelets( lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept { - lanelet::ConstLanelets shared; + lanelet::ConstLanelets linestring_shared; auto lanelet_at_left = getLeftLanelet(lane); auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); while (lanelet_at_left) { - shared.push_back(lanelet_at_left.get()); + linestring_shared.push_back(lanelet_at_left.get()); lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.get()); } if (!lanelet_at_left_opposite.empty() && include_opposite) { - shared.push_back(lanelet_at_left_opposite.front()); + linestring_shared.push_back(lanelet_at_left_opposite.front()); auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); while (lanelet_at_right) { - shared.push_back(lanelet_at_right.get()); + linestring_shared.push_back(lanelet_at_right.get()); lanelet_at_right = getRightLanelet(lanelet_at_right.get()); } } - return shared; + return linestring_shared; } lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( const lanelet::ConstLanelet & lane, const bool & include_opposite) const noexcept { - lanelet::ConstLanelets shared; + lanelet::ConstLanelets linestring_shared; auto lanelet_at_right = getRightLanelet(lane); auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); while (lanelet_at_right) { - shared.push_back(lanelet_at_right.get()); + linestring_shared.push_back(lanelet_at_right.get()); lanelet_at_right = getRightLanelet(lanelet_at_right.get()); lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.get()); } if (!lanelet_at_right_opposite.empty() && include_opposite) { - shared.push_back(lanelet_at_right_opposite.front()); + linestring_shared.push_back(lanelet_at_right_opposite.front()); auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); while (lanelet_at_left) { - shared.push_back(lanelet_at_left.get()); + linestring_shared.push_back(lanelet_at_left.get()); lanelet_at_left = getLeftLanelet(lanelet_at_left.get()); } } - return shared; + return linestring_shared; } lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( From b6ae8869bb04c63523bf8664fd96404b70b448b8 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar bin Azmi Date: Fri, 11 Mar 2022 16:13:07 +0900 Subject: [PATCH 9/9] refactor: make a wrapper function to get furthest linestrings Also modified some avoidance_module to include the refactored functions Signed-off-by: Muhammad Zulfaqar Azmi --- .../avoidance/avoidance_module.cpp | 41 ++++++------------- .../include/route_handler/route_handler.hpp | 13 ++++++ planning/route_handler/src/route_handler.cpp | 25 +++++++++++ 3 files changed, 51 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index bbe7eb54bfeac..d9239591149de 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -241,38 +241,23 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects( lanelet::BasicPoint3d overhang_basic_pose( object_data.overhang_pose.position.x, object_data.overhang_pose.position.y, object_data.overhang_pose.position.z); + const bool get_left = + isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + const bool get_right = + !isOnRight(object_data) && parameters_.enable_avoidance_over_same_direction; + + const auto target_lines = rh->getFurthestLinestring( + overhang_lanelet, get_right, get_left, + parameters_.enable_avoidance_over_opposite_direction); + if (isOnRight(object_data)) { - const auto & target_left_line = [this, &rh, &overhang_lanelet]() { - if ( - parameters_.enable_avoidance_over_same_direction && - parameters_.enable_avoidance_over_opposite_direction) { - return rh->getLeftMostLinestring(overhang_lanelet); - } else if ( - parameters_.enable_avoidance_over_same_direction && - !parameters_.enable_avoidance_over_opposite_direction) { - return rh->getLeftMostSameDirectionLinestring(overhang_lanelet); - } - return overhang_lanelet.leftBound(); - }(); object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_left_line.basicLineString())); - debug_linestring.push_back(target_left_line); + distance2d(to2D(overhang_basic_pose), to2D(target_lines.back().basicLineString())); + debug_linestring.push_back(target_lines.back()); } else { - const auto & target_right_line = [this, &rh, &overhang_lanelet]() { - if ( - parameters_.enable_avoidance_over_same_direction && - parameters_.enable_avoidance_over_opposite_direction) { - return rh->getRightMostLinestring(overhang_lanelet); - } else if ( - parameters_.enable_avoidance_over_same_direction && - !parameters_.enable_avoidance_over_opposite_direction) { - return rh->getRightMostSameDirectionLinestring(overhang_lanelet); - } - return overhang_lanelet.rightBound(); - }(); object_data.to_road_shoulder_distance = - distance2d(to2D(overhang_basic_pose), to2D(target_right_line.basicLineString())); - debug_linestring.push_back(target_right_line); + distance2d(to2D(overhang_basic_pose), to2D(target_lines.front().basicLineString())); + debug_linestring.push_back(target_lines.front()); } } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7db4f225aa813..8b6f35b86b5a9 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -203,6 +203,19 @@ class RouteHandler */ lanelet::ConstLineString3d getLeftMostLinestring( const lanelet::ConstLanelet & lanelet) const noexcept; + + /** + * @brief Return furthest linestring on both side of the lanelet + * @param the lanelet of interest + * @param (optional) search furthest right side + * @param (optional) search furthest left side + * @param (optional) include opposite lane as well + * @return right and left linestrings + */ + lanelet::ConstLineStrings3d getFurthestLinestring( + const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, + bool is_opposite = true) const noexcept; + int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 2b47a6857b649..406e070f31c4b 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1039,6 +1039,31 @@ lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring( return {}; } +lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( + const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, + bool is_opposite) const noexcept +{ + lanelet::ConstLineStrings3d linestrings; + linestrings.reserve(2); + + if (is_right && is_opposite) { + linestrings.emplace_back(getRightMostLinestring(lanelet)); + } else if (is_right && !is_opposite) { + linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet)); + } else { + linestrings.emplace_back(lanelet.rightBound()); + } + + if (is_left && is_opposite) { + linestrings.emplace_back(getLeftMostLinestring(lanelet)); + } else if (is_left && !is_opposite) { + linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet)); + } else { + linestrings.emplace_back(lanelet.leftBound()); + } + return linestrings; +} + bool RouteHandler::getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const {