From 7e098087b0ebf4d9b45318282ec65abe4b478db3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 18 Mar 2024 11:16:13 +0900 Subject: [PATCH] fix(route_handler): detect looped road shoulders in getShoulderLaneletSequence() (#6633) Signed-off-by: Mamoru Sobue --- .../src/goal_planner_module.cpp | 2 ++ planning/route_handler/src/route_handler.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index ce0fc8cfbece1..c2902fb0bace6 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1856,6 +1856,8 @@ bool GoalPlannerModule::isCrossingPossible( Pose end_lane_pose{}; end_lane_pose.orientation.w = 1.0; end_lane_pose.position = lanelet::utils::conversion::toGeomMsgPt(end_lane.centerline().front()); + // NOTE: this line does not specify the /forward/backward length, so if the shoulders form a + // loop, this returns all shoulder lanes in the loop end_lane_sequence = route_handler->getShoulderLaneletSequence(end_lane, end_lane_pose); } else { const double dist = std::numeric_limits::max(); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 02a48580e0301..8cec5073efb6a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -758,12 +758,18 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter( double length = 0; lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getFollowingShoulderLanelet(current_lanelet, &next_lanelet)) { break; } lanelet_sequence_forward.push_back(next_lanelet); + if (searched_ids.find(next_lanelet.id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(next_lanelet.id()); current_lanelet = next_lanelet; length += static_cast(boost::geometry::length(next_lanelet.centerline().basicLineString())); @@ -794,6 +800,7 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( double length = 0; lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet prev_lanelet; if (!getPreviousShoulderLanelet(current_lanelet, &prev_lanelet)) { @@ -801,6 +808,11 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( } lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), prev_lanelet); + if (searched_ids.find(prev_lanelet.id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(prev_lanelet.id()); current_lanelet = prev_lanelet; length += static_cast(boost::geometry::length(prev_lanelet.centerline().basicLineString()));