diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 01e52d85f01c3..89930fd4a23a9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -1163,32 +1164,40 @@ lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_left = getLeftLanelet(lane); - auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); - while (lanelet_at_left) { - linestring_shared.push_back(lanelet_at_left.value()); - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); - if (!lanelet_at_left) { - break; + try { + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.value()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); + if (!lanelet_at_left) { + break; + } + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); } - lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); - } - if (!lanelet_at_left_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_left_opposite.front()); - } - auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); - while (lanelet_at_right) { + if (!lanelet_at_left_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right.value().invert()); + linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_right.value()); + linestring_shared.push_back(lanelet_at_left_opposite.front()); + } + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_right.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_right.value()); + } + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllLeftSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllLeftSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; } @@ -1198,32 +1207,40 @@ lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_right = getRightLanelet(lane); - auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); - while (lanelet_at_right) { - linestring_shared.push_back(lanelet_at_right.value()); - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); - if (!lanelet_at_right) { - break; + try { + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.value()); + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); + if (!lanelet_at_right) { + break; + } + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); } - lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); - } - if (!lanelet_at_right_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_right_opposite.front()); - } - auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); - while (lanelet_at_left) { + if (!lanelet_at_right_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left.value().invert()); + linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_left.value()); + linestring_shared.push_back(lanelet_at_right_opposite.front()); + } + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_left.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_left.value()); + } + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllRightSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllRightSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; }