@@ -943,6 +943,44 @@ double calcShiftLength(
943
943
return std::fabs (shift_length) > 1e-3 ? shift_length : 0.0 ;
944
944
}
945
945
946
+ bool isWithinLanes (
947
+ const lanelet::ConstLanelets & lanelets, std::shared_ptr<const PlannerData> & planner_data)
948
+ {
949
+ const auto & rh = planner_data->route_handler ;
950
+ const auto & ego_pose = planner_data->self_odometry ->pose .pose ;
951
+ const auto transform = tier4_autoware_utils::pose2transform (ego_pose);
952
+ const auto footprint = tier4_autoware_utils::transformVector (
953
+ planner_data->parameters .vehicle_info .createFootprint (), transform);
954
+
955
+ lanelet::ConstLanelet closest_lanelet{};
956
+ if (!lanelet::utils::query::getClosestLanelet (lanelets, ego_pose, &closest_lanelet)) {
957
+ return true ;
958
+ }
959
+
960
+ lanelet::ConstLanelets concat_lanelets{};
961
+
962
+ // push previous lanelet
963
+ lanelet::ConstLanelets prev_lanelet;
964
+ if (rh->getPreviousLaneletsWithinRoute (closest_lanelet, &prev_lanelet)) {
965
+ concat_lanelets.push_back (prev_lanelet.front ());
966
+ }
967
+
968
+ // push nearest lanelet
969
+ {
970
+ concat_lanelets.push_back (closest_lanelet);
971
+ }
972
+
973
+ // push next lanelet
974
+ lanelet::ConstLanelet next_lanelet;
975
+ if (rh->getNextLaneletWithinRoute (closest_lanelet, &next_lanelet)) {
976
+ concat_lanelets.push_back (next_lanelet);
977
+ }
978
+
979
+ const auto combine_lanelet = lanelet::utils::combineLaneletsShape (concat_lanelets);
980
+
981
+ return boost::geometry::within (footprint, combine_lanelet.polygon2d ().basicPolygon ());
982
+ }
983
+
946
984
bool isShiftNecessary (const bool & is_object_on_right, const double & shift_length)
947
985
{
948
986
/* *
0 commit comments