From 0c4771e79be006a0a440e2d397a2393b336109bc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 8 Mar 2024 00:35:42 +0900 Subject: [PATCH] fix(route_handler): query shoulder lane in getLaneletSequence() if only_route_lanes is false and the arg is shoulder type (#6567) 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 68be0ac33e934..c462b3646e9fe 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{}; });