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(