From a6508cc0c9f5f9fbfeb8ed275997e6b49a8a049e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 7 Mar 2024 21:00:38 +0900 Subject: [PATCH] fix(route_handle): query shoulder lane in getLaneletSequence() if only_route_lanes is false and the arg is shoulder Signed-off-by: Mamoru Sobue --- planning/route_handler/src/route_handler.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f7d300d496e13..02a48580e0301 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -643,12 +643,22 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( return lanelet_sequence; } - lanelet::ConstLanelets lanelet_sequence_forward = - getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() { + if (only_route_lanes) { + return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceAfter(lanelet, forward_distance); + } + return lanelet::ConstLanelets{}; + }); const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { - return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + if (only_route_lanes) { + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceUpTo(lanelet, backward_distance); + } } return lanelet::ConstLanelets{}; });