From b1e0ff513a74c6db763d5037a5c3b9602e72518f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 16 Sep 2022 20:03:05 +0900 Subject: [PATCH] fix(route_handler): deal with completely overlapped inverted lane Signed-off-by: Takayuki Murooka --- planning/route_handler/src/route_handler.cpp | 28 ++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 621365eda0e7c..6106fa86c1f59 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -865,7 +865,19 @@ boost::optional RouteHandler::getLeftLanelet( lanelet::Lanelets RouteHandler::getRightOppositeLanelets( const lanelet::ConstLanelet & lanelet) const { - return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; } lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( @@ -943,7 +955,19 @@ lanelet::ConstLanelets RouteHandler::getAllSharedLineStringLanelets( lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLanelet & lanelet) const { - return lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); + const auto opposite_candidate_lanelets = + lanelet_map_ptr_->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; } lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring(