29
29
#include < algorithm>
30
30
#include < array>
31
31
#include < random>
32
+ #include < utility>
32
33
33
34
namespace
34
35
{
@@ -172,6 +173,8 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha
172
173
void MissionPlanner::on_map (const HADMapBin::ConstSharedPtr msg)
173
174
{
174
175
map_ptr_ = msg;
176
+ lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
177
+ lanelet::utils::conversion::fromBinMsg (*map_ptr_, lanelet_map_ptr_);
175
178
}
176
179
177
180
PoseStamped MissionPlanner::transform_pose (const PoseStamped & input)
@@ -730,7 +733,10 @@ void MissionPlanner::on_change_route_points(
730
733
bool MissionPlanner::check_reroute_safety (
731
734
const LaneletRoute & original_route, const LaneletRoute & target_route)
732
735
{
733
- if (original_route.segments .empty () || target_route.segments .empty () || !map_ptr_ || !odometry_) {
736
+ if (
737
+ original_route.segments .empty () || target_route.segments .empty () || !map_ptr_ ||
738
+ !lanelet_map_ptr_ || !odometry_) {
739
+ RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Route, map or odometry is not set." );
734
740
return false ;
735
741
}
736
742
@@ -748,109 +754,135 @@ bool MissionPlanner::check_reroute_safety(
748
754
return false ;
749
755
}
750
756
751
- bool is_same = false ;
752
757
for (const auto & primitive : original_primitives) {
753
758
const auto has_same = [&](const auto & p) { return p.id == primitive.id ; };
754
- is_same = std::find_if (target_primitives.begin (), target_primitives.end (), has_same) !=
755
- target_primitives.end ();
756
- }
757
- return is_same;
758
- };
759
-
760
- // find idx of original primitives that matches the target primitives
761
- const auto start_idx_opt = std::invoke ([&]() -> std::optional<size_t > {
762
- /*
763
- * find the index of the original route that has same idx with the front segment of the new
764
- * route
765
- *
766
- * start_idx
767
- * +-----------+-----------+-----------+-----------+-----------+
768
- * | | | | | |
769
- * +-----------+-----------+-----------+-----------+-----------+
770
- * | | | | | |
771
- * +-----------+-----------+-----------+-----------+-----------+
772
- * original original original original original
773
- * target target target
774
- */
775
- const auto target_front_primitives = target_route.segments .front ().primitives ;
776
- for (size_t i = 0 ; i < original_route.segments .size (); ++i) {
777
- const auto & original_primitives = original_route.segments .at (i).primitives ;
778
- if (hasSamePrimitives (original_primitives, target_front_primitives)) {
779
- return i;
759
+ const bool is_same =
760
+ std::find_if (target_primitives.begin (), target_primitives.end (), has_same) !=
761
+ target_primitives.end ();
762
+ if (!is_same) {
763
+ return false ;
780
764
}
781
765
}
766
+ return true ;
767
+ };
782
768
783
- /*
784
- * find the target route that has same idx with the front segment of the original route
785
- *
786
- * start_idx
787
- * +-----------+-----------+-----------+-----------+-----------+
788
- * | | | | | |
789
- * +-----------+-----------+-----------+-----------+-----------+
790
- * | | | | | |
791
- * +-----------+-----------+-----------+-----------+-----------+
792
- * original original original
793
- * target target target target target
794
- */
795
- const auto original_front_primitives = original_route.segments .front ().primitives ;
796
- for (size_t i = 0 ; i < target_route.segments .size (); ++i) {
797
- const auto & target_primitives = target_route.segments .at (i).primitives ;
798
- if (hasSamePrimitives (target_primitives, original_front_primitives)) {
799
- return 0 ;
769
+ // =============================================================================================
770
+ // NOTE: the target route is calculated while ego is driving on the original route, so basically
771
+ // the first lane of the target route should be in the original route lanelets. So the common
772
+ // segment interval matches the beginning of the target route. The exception is that if ego is
773
+ // on an intersection lanelet, getClosestLanelet() may not return the same lanelet which exists
774
+ // in the original route. In that case the common segment interval does not match the beginning of
775
+ // the target lanelet
776
+ // =============================================================================================
777
+ const auto start_idx_opt =
778
+ std::invoke ([&]() -> std::optional<std::pair<size_t /* original */ , size_t /* target */ >> {
779
+ for (size_t i = 0 ; i < original_route.segments .size (); ++i) {
780
+ const auto & original_segment = original_route.segments .at (i).primitives ;
781
+ for (size_t j = 0 ; j < target_route.segments .size (); ++j) {
782
+ const auto & target_segment = target_route.segments .at (j).primitives ;
783
+ if (hasSamePrimitives (original_segment, target_segment)) {
784
+ return std::make_pair (i, j);
785
+ }
786
+ }
800
787
}
801
- }
802
-
803
- return std::nullopt;
804
- });
788
+ return std::nullopt;
789
+ });
805
790
if (!start_idx_opt.has_value ()) {
791
+ RCLCPP_ERROR (
792
+ get_logger (), " Check reroute safety failed. Cannot find the start index of the route." );
806
793
return false ;
807
794
}
808
- const size_t start_idx = start_idx_opt.value ();
795
+ const auto [start_idx_original, start_idx_target] = start_idx_opt.value ();
809
796
810
797
// find last idx that matches the target primitives
811
- size_t end_idx = start_idx;
812
- for (size_t i = 1 ; i < target_route.segments .size (); ++i) {
813
- if (start_idx + i > original_route.segments .size () - 1 ) {
798
+ size_t end_idx_original = start_idx_original;
799
+ size_t end_idx_target = start_idx_target;
800
+ for (size_t i = 1 ; i < target_route.segments .size () - start_idx_target; ++i) {
801
+ if (start_idx_original + i > original_route.segments .size () - 1 ) {
814
802
break ;
815
803
}
816
804
817
- const auto & original_primitives = original_route.segments .at (start_idx + i).primitives ;
818
- const auto & target_primitives = target_route.segments .at (i).primitives ;
805
+ const auto & original_primitives =
806
+ original_route.segments .at (start_idx_original + i).primitives ;
807
+ const auto & target_primitives = target_route.segments .at (start_idx_target + i).primitives ;
819
808
if (!hasSamePrimitives (original_primitives, target_primitives)) {
820
809
break ;
821
810
}
822
- end_idx = start_idx + i;
811
+ end_idx_original = start_idx_original + i;
812
+ end_idx_target = start_idx_target + i;
813
+ }
814
+
815
+ // at the very first transition from main/MRM to MRM/main, the requested route from the
816
+ // route_selector may not begin from ego current lane (because route_selector requests the
817
+ // previous route once, and then replan)
818
+ const bool ego_is_on_first_target_section = std::any_of (
819
+ target_route.segments .front ().primitives .begin (),
820
+ target_route.segments .front ().primitives .end (), [&](const auto & primitive) {
821
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
822
+ return lanelet::utils::isInLanelet (target_route.start_pose , lanelet);
823
+ });
824
+ if (!ego_is_on_first_target_section) {
825
+ RCLCPP_ERROR (
826
+ get_logger (),
827
+ " Check reroute safety failed. Ego is not on the first section of target route." );
828
+ return false ;
823
829
}
824
830
825
- // create map
826
- auto lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
827
- lanelet::utils::conversion::fromBinMsg (*map_ptr_, lanelet_map_ptr_);
831
+ // if the front of target route is not the front of common segment, it is expected that the front
832
+ // of the target route is conflicting with another lane which is equal to original
833
+ // route[start_idx_original-1]
834
+ double accumulated_length = 0.0 ;
828
835
829
- // compute distance from the current pose to the end of the current lanelet
830
- const auto current_pose = target_route.start_pose ;
831
- const auto primitives = original_route.segments .at (start_idx).primitives ;
832
- lanelet::ConstLanelets start_lanelets;
833
- for (const auto & primitive : primitives) {
834
- const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
835
- start_lanelets.push_back (lanelet);
836
- }
836
+ if (start_idx_target != 0 && start_idx_original > 1 ) {
837
+ // compute distance from the current pose to the beginning of the common segment
838
+ const auto current_pose = target_route.start_pose ;
839
+ const auto primitives = original_route.segments .at (start_idx_original - 1 ).primitives ;
840
+ lanelet::ConstLanelets start_lanelets;
841
+ for (const auto & primitive : primitives) {
842
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
843
+ start_lanelets.push_back (lanelet);
844
+ }
845
+ // closest lanelet in start lanelets
846
+ lanelet::ConstLanelet closest_lanelet;
847
+ if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
848
+ RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Cannot find the closest lanelet." );
849
+ return false ;
850
+ }
837
851
838
- // get closest lanelet in start lanelets
839
- lanelet::ConstLanelet closest_lanelet;
840
- if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
841
- return false ;
842
- }
852
+ const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
853
+ const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
854
+ const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
855
+ centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
856
+ const double dist_to_current_pose = arc_coordinates.length ;
857
+ const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
858
+ accumulated_length = lanelet_length - dist_to_current_pose;
859
+ } else {
860
+ // compute distance from the current pose to the end of the current lanelet
861
+ const auto current_pose = target_route.start_pose ;
862
+ const auto primitives = original_route.segments .at (start_idx_original).primitives ;
863
+ lanelet::ConstLanelets start_lanelets;
864
+ for (const auto & primitive : primitives) {
865
+ const auto lanelet = lanelet_map_ptr_->laneletLayer .get (primitive.id );
866
+ start_lanelets.push_back (lanelet);
867
+ }
868
+ // closest lanelet in start lanelets
869
+ lanelet::ConstLanelet closest_lanelet;
870
+ if (!lanelet::utils::query::getClosestLanelet (start_lanelets, current_pose, &closest_lanelet)) {
871
+ RCLCPP_ERROR (get_logger (), " Check reroute safety failed. Cannot find the closest lanelet." );
872
+ return false ;
873
+ }
843
874
844
- const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
845
- const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
846
- const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
847
- centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
848
- const double dist_to_current_pose = arc_coordinates.length ;
849
- const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
850
- double accumulated_length = lanelet_length - dist_to_current_pose;
875
+ const auto & centerline_2d = lanelet::utils::to2D (closest_lanelet.centerline ());
876
+ const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint (current_pose.position );
877
+ const auto arc_coordinates = lanelet::geometry::toArcCoordinates (
878
+ centerline_2d, lanelet::utils::to2D (lanelet_point).basicPoint ());
879
+ const double dist_to_current_pose = arc_coordinates.length ;
880
+ const double lanelet_length = lanelet::utils::getLaneletLength2d (closest_lanelet);
881
+ accumulated_length = lanelet_length - dist_to_current_pose;
882
+ }
851
883
852
884
// compute distance from the start_idx+1 to end_idx
853
- for (size_t i = start_idx + 1 ; i <= end_idx ; ++i) {
885
+ for (size_t i = start_idx_original + 1 ; i <= end_idx_original ; ++i) {
854
886
const auto primitives = original_route.segments .at (i).primitives ;
855
887
if (primitives.empty ()) {
856
888
break ;
@@ -866,7 +898,7 @@ bool MissionPlanner::check_reroute_safety(
866
898
}
867
899
868
900
// check if the goal is inside of the target terminal lanelet
869
- const auto & target_end_primitives = target_route.segments .at (end_idx - start_idx ).primitives ;
901
+ const auto & target_end_primitives = target_route.segments .at (end_idx_target ).primitives ;
870
902
const auto & target_goal = target_route.goal_pose ;
871
903
for (const auto & target_end_primitive : target_end_primitives) {
872
904
const auto lanelet = lanelet_map_ptr_->laneletLayer .get (target_end_primitive.id );
@@ -878,8 +910,11 @@ bool MissionPlanner::check_reroute_safety(
878
910
lanelet::utils::to2D (target_goal_position).basicPoint ())
879
911
.length ;
880
912
const double target_lanelet_length = lanelet::utils::getLaneletLength2d (lanelet);
881
- const double dist = target_lanelet_length - dist_to_goal;
882
- accumulated_length = std::max (accumulated_length - dist, 0.0 );
913
+ // NOTE: `accumulated_length` here contains the length of the entire target_end_primitive, so
914
+ // the remaining distance from the goal to the end of the target_end_primitive needs to be
915
+ // subtracted.
916
+ const double remaining_dist = target_lanelet_length - dist_to_goal;
917
+ accumulated_length = std::max (accumulated_length - remaining_dist, 0.0 );
883
918
break ;
884
919
}
885
920
}
0 commit comments