Skip to content

Commit 7e09808

Browse files
authored
fix(route_handler): detect looped road shoulders in getShoulderLaneletSequence() (#6633)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 546df44 commit 7e09808

File tree

2 files changed

+14
-0
lines changed

2 files changed

+14
-0
lines changed

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -1856,6 +1856,8 @@ bool GoalPlannerModule::isCrossingPossible(
18561856
Pose end_lane_pose{};
18571857
end_lane_pose.orientation.w = 1.0;
18581858
end_lane_pose.position = lanelet::utils::conversion::toGeomMsgPt(end_lane.centerline().front());
1859+
// NOTE: this line does not specify the /forward/backward length, so if the shoulders form a
1860+
// loop, this returns all shoulder lanes in the loop
18591861
end_lane_sequence = route_handler->getShoulderLaneletSequence(end_lane, end_lane_pose);
18601862
} else {
18611863
const double dist = std::numeric_limits<double>::max();

planning/route_handler/src/route_handler.cpp

+12
Original file line numberDiff line numberDiff line change
@@ -758,12 +758,18 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter(
758758

759759
double length = 0;
760760
lanelet::ConstLanelet current_lanelet = lanelet;
761+
std::set<lanelet::Id> searched_ids{};
761762
while (rclcpp::ok() && length < min_length) {
762763
lanelet::ConstLanelet next_lanelet;
763764
if (!getFollowingShoulderLanelet(current_lanelet, &next_lanelet)) {
764765
break;
765766
}
766767
lanelet_sequence_forward.push_back(next_lanelet);
768+
if (searched_ids.find(next_lanelet.id()) != searched_ids.end()) {
769+
// loop shoulder detected
770+
break;
771+
}
772+
searched_ids.insert(next_lanelet.id());
767773
current_lanelet = next_lanelet;
768774
length +=
769775
static_cast<double>(boost::geometry::length(next_lanelet.centerline().basicLineString()));
@@ -794,13 +800,19 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo(
794800

795801
double length = 0;
796802
lanelet::ConstLanelet current_lanelet = lanelet;
803+
std::set<lanelet::Id> searched_ids{};
797804
while (rclcpp::ok() && length < min_length) {
798805
lanelet::ConstLanelet prev_lanelet;
799806
if (!getPreviousShoulderLanelet(current_lanelet, &prev_lanelet)) {
800807
break;
801808
}
802809

803810
lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), prev_lanelet);
811+
if (searched_ids.find(prev_lanelet.id()) != searched_ids.end()) {
812+
// loop shoulder detected
813+
break;
814+
}
815+
searched_ids.insert(prev_lanelet.id());
804816
current_lanelet = prev_lanelet;
805817
length +=
806818
static_cast<double>(boost::geometry::length(prev_lanelet.centerline().basicLineString()));

0 commit comments

Comments
 (0)