@@ -573,44 +573,26 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo(
573
573
574
574
lanelet::ConstLanelet current_lanelet = lanelet;
575
575
double length = 0 ;
576
+ lanelet::ConstLanelets previous_lanelets;
576
577
while (rclcpp::ok () && length < min_length) {
577
- lanelet::ConstLanelets candidate_lanelets;
578
- if (!getPreviousLaneletsWithinRoute (current_lanelet, &candidate_lanelets)) {
579
- if (only_route_lanes) {
580
- break ;
581
- }
582
- const auto prev_lanes = getPreviousLanelets (current_lanelet);
583
- if (prev_lanes.empty ()) {
584
- break ;
585
- }
586
- candidate_lanelets = prev_lanes;
578
+ previous_lanelets.clear ();
579
+ if (!getPreviousLaneletsWithinRoute (current_lanelet, &previous_lanelets)) {
580
+ if (only_route_lanes) break ;
581
+ const auto previous_lanelets = getPreviousLanelets (current_lanelet);
582
+ if (previous_lanelets.empty ()) break ;
587
583
}
588
584
// loop check
589
- if (std::any_of (
590
- candidate_lanelets.begin (), candidate_lanelets.end (),
591
- [lanelet](auto & prev_llt) { return lanelet.id () == prev_llt.id (); })) {
592
- break ;
593
- }
594
-
595
- // If lanelet_sequence_backward with input lanelet contains all candidate lanelets,
596
- // break the loop.
597
- if (std::all_of (
598
- candidate_lanelets.begin (), candidate_lanelets.end (),
599
- [lanelet_sequence_backward, lanelet](auto & prev_llt) {
600
- return std::any_of (
601
- lanelet_sequence_backward.begin (), lanelet_sequence_backward.end (),
602
- [prev_llt, lanelet](auto & llt) {
603
- return (llt.id () == prev_llt.id () || lanelet.id () == prev_llt.id ());
604
- });
605
- })) {
585
+ if (std::any_of (previous_lanelets.begin (), previous_lanelets.end (), [lanelet](auto & prev_llt) {
586
+ return lanelet.id () == prev_llt.id ();
587
+ })) {
606
588
break ;
607
589
}
608
590
609
- for (const auto & prev_lanelet : candidate_lanelets ) {
591
+ for (const auto & prev_lanelet : previous_lanelets ) {
610
592
if (std::any_of (
611
593
lanelet_sequence_backward.begin (), lanelet_sequence_backward.end (),
612
- [prev_lanelet, lanelet](auto & llt ) {
613
- return (llt. id () == prev_lanelet. id () || lanelet .id () == prev_lanelet.id ());
594
+ [prev_lanelet, lanelet](auto & backward ) {
595
+ return (backward .id () == prev_lanelet.id ());
614
596
})) {
615
597
continue ;
616
598
}
@@ -680,14 +662,13 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
680
662
const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance,
681
663
const double forward_distance, const bool only_route_lanes) const
682
664
{
683
- lanelet::ConstLanelets lanelet_sequence;
684
665
if (only_route_lanes && !exists (route_lanelets_, lanelet)) {
685
- return lanelet_sequence ;
666
+ return {} ;
686
667
}
687
668
688
669
lanelet::ConstLanelets lanelet_sequence_forward =
689
670
getLaneletSequenceAfter (lanelet, forward_distance, only_route_lanes);
690
- const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke ([&]() {
671
+ lanelet::ConstLanelets lanelet_sequence = std::invoke ([&]() {
691
672
const auto arc_coordinate = lanelet::utils::getArcCoordinates ({lanelet}, current_pose);
692
673
if (arc_coordinate.length < backward_distance) {
693
674
return getLaneletSequenceUpTo (lanelet, backward_distance, only_route_lanes);
@@ -696,17 +677,15 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(
696
677
});
697
678
698
679
// loop check
699
- if (!lanelet_sequence_forward.empty () && !lanelet_sequence_backward .empty ()) {
700
- if (lanelet_sequence_backward .back ().id () == lanelet_sequence_forward.front ().id ()) {
680
+ if (!lanelet_sequence_forward.empty () && !lanelet_sequence .empty ()) {
681
+ if (lanelet_sequence .back ().id () == lanelet_sequence_forward.front ().id ()) {
701
682
return lanelet_sequence_forward;
702
683
}
703
684
}
704
- lanelet_sequence.insert (
705
- lanelet_sequence.end (), lanelet_sequence_backward.begin (), lanelet_sequence_backward.end ());
706
685
lanelet_sequence.push_back (lanelet);
707
- lanelet_sequence. insert (
708
- lanelet_sequence. end (), lanelet_sequence_forward.begin (), lanelet_sequence_forward.end ());
709
-
686
+ std::move (
687
+ lanelet_sequence_forward.begin (), lanelet_sequence_forward.end (),
688
+ std::back_inserter (lanelet_sequence));
710
689
return lanelet_sequence;
711
690
}
712
691
@@ -1681,8 +1660,9 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo(
1681
1660
}
1682
1661
1683
1662
lanelet::ConstLanelet current_lanelet = lanelet;
1663
+ lanelet::ConstLanelets candidate_lanelets;
1684
1664
while (rclcpp::ok ()) {
1685
- lanelet::ConstLanelets candidate_lanelets;
1665
+ candidate_lanelets. clear () ;
1686
1666
if (!getPreviousLaneletsWithinRoute (current_lanelet, &candidate_lanelets)) {
1687
1667
break ;
1688
1668
}
0 commit comments