@@ -758,12 +758,18 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter(
758
758
759
759
double length = 0 ;
760
760
lanelet::ConstLanelet current_lanelet = lanelet;
761
+ std::set<lanelet::Id> searched_ids{};
761
762
while (rclcpp::ok () && length < min_length) {
762
763
lanelet::ConstLanelet next_lanelet;
763
764
if (!getFollowingShoulderLanelet (current_lanelet, &next_lanelet)) {
764
765
break ;
765
766
}
766
767
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 ());
767
773
current_lanelet = next_lanelet;
768
774
length +=
769
775
static_cast <double >(boost::geometry::length (next_lanelet.centerline ().basicLineString ()));
@@ -794,13 +800,19 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo(
794
800
795
801
double length = 0 ;
796
802
lanelet::ConstLanelet current_lanelet = lanelet;
803
+ std::set<lanelet::Id> searched_ids{};
797
804
while (rclcpp::ok () && length < min_length) {
798
805
lanelet::ConstLanelet prev_lanelet;
799
806
if (!getPreviousShoulderLanelet (current_lanelet, &prev_lanelet)) {
800
807
break ;
801
808
}
802
809
803
810
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 ());
804
816
current_lanelet = prev_lanelet;
805
817
length +=
806
818
static_cast <double >(boost::geometry::length (prev_lanelet.centerline ().basicLineString ()));
0 commit comments