diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp index ae4e08774f679..a547971da1820 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/data_structure.hpp @@ -15,17 +15,24 @@ #ifndef MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ #define MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ +#include + +#include +#include #include +#include +#include #include #include #include #include #include +#include #include +#include #include - namespace autoware::map_based_prediction { using PosePath = std::vector; @@ -58,7 +65,7 @@ enum class Maneuver { struct LaneletData { lanelet::Lanelet lanelet; - float probability; + double probability; }; struct PredictedRefPath @@ -90,6 +97,23 @@ struct CrosswalkUserData autoware_perception_msgs::msg::TrackedObject tracked_object; }; +using LaneletsData = std::vector; +using ManeuverProbability = std::unordered_map; +using autoware::universe_utils::StopWatch; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjectKinematics; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; +using LaneletPathWithPathInfo = std::pair; + } // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__DATA_STRUCTURE_HPP_ diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 0e51fea6ac739..1675f8b1fbfa2 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -25,16 +25,10 @@ #include #include #include -#include #include -#include #include #include -#include -#include -#include -#include #include #include #include @@ -77,25 +71,10 @@ struct hash } // namespace std namespace autoware::map_based_prediction { -using LaneletsData = std::vector; -using ManeuverProbability = std::unordered_map; -using autoware::universe_utils::StopWatch; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::ObjectClassification; -using autoware_perception_msgs::msg::PredictedObject; -using autoware_perception_msgs::msg::PredictedObjectKinematics; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_perception_msgs::msg::PredictedPath; -using autoware_perception_msgs::msg::TrackedObject; -using autoware_perception_msgs::msg::TrackedObjectKinematics; -using autoware_perception_msgs::msg::TrackedObjects; -using autoware_perception_msgs::msg::TrafficLightElement; -using autoware_perception_msgs::msg::TrafficLightGroup; -using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; -using LaneletPathWithPathInfo = std::pair; + class MapBasedPredictionNode : public rclcpp::Node { public: @@ -202,10 +181,6 @@ class MapBasedPredictionNode : public rclcpp::Node //// Vehicle process // Lanelet process LaneletsData getCurrentLanelets(const TrackedObject & object); - bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object); - float calculateLocalLikelihood( - const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; bool isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data); diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp index 06aab91c178eb..26a3b46564e30 100644 --- a/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/utils.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -94,6 +95,39 @@ PredictedObjectKinematics convertToPredictedKinematics( PredictedObject convertToPredictedObject(const TrackedObject & tracked_object); +double calculateLocalLikelihood( + const lanelet::Lanelet & current_lanelet, const TrackedObject & object, + const double sigma_lateral_offset, const double sigma_yaw_angle_deg); + +bool isDuplicated( + const std::pair & target_lanelet, + const LaneletsData & lanelets_data); + +bool isDuplicated( + const PredictedPath & predicted_path, const std::vector & predicted_paths); + +bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const std::unordered_map> & road_users_history, + const double dist_threshold_for_searching_lanelet, + const double delta_yaw_threshold_for_searching_lanelet); + +// NOTE: These two functions are copied from the route_handler package. +lanelet::Lanelets getRightOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet); + +lanelet::Lanelets getLeftOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet); + +LaneletsData getCurrentLanelets( + const TrackedObject & object, lanelet::LaneletMapPtr lanelet_map_ptr, + const std::unordered_map> & road_users_history, + const double dist_threshold_for_searching_lanelet, + const double delta_yaw_threshold_for_searching_lanelet, const double sigma_lateral_offset, + const double sigma_yaw_angle_deg); + } // namespace utils } // namespace autoware::map_based_prediction diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 119cfececab95..4c4b7b30caecf 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -324,45 +325,6 @@ lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & da return lanelets; } -// NOTE: These two functions are copied from the route_handler package. -lanelet::Lanelets getRightOppositeLanelets( - const std::shared_ptr & lanelet_map_ptr, - const lanelet::ConstLanelet & lanelet) -{ - const auto opposite_candidate_lanelets = - lanelet_map_ptr->laneletLayer.findUsages(lanelet.rightBound().invert()); - - lanelet::Lanelets opposite_lanelets; - for (const auto & candidate_lanelet : opposite_candidate_lanelets) { - if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { - continue; - } - - opposite_lanelets.push_back(candidate_lanelet); - } - - return opposite_lanelets; -} - -lanelet::Lanelets getLeftOppositeLanelets( - const std::shared_ptr & lanelet_map_ptr, - const lanelet::ConstLanelet & lanelet) -{ - const auto opposite_candidate_lanelets = - lanelet_map_ptr->laneletLayer.findUsages(lanelet.leftBound().invert()); - - lanelet::Lanelets opposite_lanelets; - for (const auto & candidate_lanelet : opposite_candidate_lanelets) { - if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { - continue; - } - - opposite_lanelets.push_back(candidate_lanelet); - } - - return opposite_lanelets; -} - void replaceObjectYawWithLaneletsYaw( const LaneletsData & current_lanelets, TrackedObject & transformed_object) { @@ -779,177 +741,9 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob std::unique_ptr st_ptr; if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - // obstacle point - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - - // nearest lanelet - std::vector> surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr_->laneletLayer, search_point, 10); - - { // Step 1. Search same directional lanelets - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return {}; - } - - LaneletsData object_lanelets; - std::optional> closest_lanelet{std::nullopt}; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets and - // Check if similar lanelet is inside the object lanelet - if (!checkCloseLaneletCondition(lanelet, object) || isDuplicated(lanelet, object_lanelets)) { - continue; - } - - // Memorize closest lanelet - // NOTE: The object may be outside the lanelet. - if (!closest_lanelet || lanelet.first < closest_lanelet->first) { - closest_lanelet = lanelet; - } - - // Check if the obstacle is inside of this lanelet - constexpr double epsilon = 1e-3; - if (lanelet.first < epsilon) { - const auto object_lanelet = - LaneletData{lanelet.second, calculateLocalLikelihood(lanelet.second, object)}; - object_lanelets.push_back(object_lanelet); - } - } - - if (!object_lanelets.empty()) { - return object_lanelets; - } - if (closest_lanelet) { - return LaneletsData{LaneletData{ - closest_lanelet->second, calculateLocalLikelihood(closest_lanelet->second, object)}}; - } - } - - { // Step 2. Search opposite directional lanelets - // Get opposite lanelets and calculate distance to search point. - std::vector> surrounding_opposite_lanelets; - for (const auto & surrounding_lanelet : surrounding_lanelets) { - for (const auto & left_opposite_lanelet : - getLeftOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { - const double distance = lanelet::geometry::distance2d(left_opposite_lanelet, search_point); - surrounding_opposite_lanelets.push_back(std::make_pair(distance, left_opposite_lanelet)); - } - for (const auto & right_opposite_lanelet : - getRightOppositeLanelets(lanelet_map_ptr_, surrounding_lanelet.second)) { - const double distance = lanelet::geometry::distance2d(right_opposite_lanelet, search_point); - surrounding_opposite_lanelets.push_back(std::make_pair(distance, right_opposite_lanelet)); - } - } - - std::optional> opposite_closest_lanelet{std::nullopt}; - for (const auto & lanelet : surrounding_opposite_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets - // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object)) { - continue; - } - - // Memorize closest lanelet - if (!opposite_closest_lanelet || lanelet.first < opposite_closest_lanelet->first) { - opposite_closest_lanelet = lanelet; - } - } - if (opposite_closest_lanelet) { - return LaneletsData{LaneletData{ - opposite_closest_lanelet->second, - calculateLocalLikelihood(opposite_closest_lanelet->second, object)}}; - } - } - - return LaneletsData{}; -} - -bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object) -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - // Step1. If we only have one point in the centerline, we will ignore the lanelet - if (lanelet.second.centerline().size() <= 1) { - return false; - } - - // If the object is in the objects history, we check if the target lanelet is - // inside the current lanelets id or following lanelets - const std::string object_id = autoware::universe_utils::toHexString(object.object_id); - if (road_users_history_.count(object_id) != 0) { - const std::vector & possible_lanelet = - road_users_history_.at(object_id).back().future_possible_lanelets; - - bool not_in_possible_lanelet = - std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == - possible_lanelet.end(); - if (!possible_lanelet.empty() && not_in_possible_lanelet) { - return false; - } - } - - // Step2. Calculate the angle difference between the lane angle and obstacle angle - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet.second, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); - const double abs_norm_delta = std::fabs(normalized_delta_yaw); - - // Step3. Check if the closest lanelet is valid, and add all - // of the lanelets that are below max_dist and max_delta_yaw - const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; - const bool is_yaw_reversed = - M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (dist_threshold_for_searching_lanelet_ < lanelet.first) { - return false; - } - if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { - return false; - } - - return true; -} - -float MapBasedPredictionNode::calculateLocalLikelihood( - const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const -{ - std::unique_ptr st_ptr; - if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - - const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; - - // compute yaw difference between the object and lane - const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); - const double delta_yaw = obj_yaw - lane_yaw; - const double abs_norm_delta_yaw = std::fabs(autoware::universe_utils::normalizeRadian(delta_yaw)); - - // compute lateral distance - const auto centerline = current_lanelet.centerline(); - std::vector converted_centerline; - for (const auto & p : centerline) { - const auto converted_p = lanelet::utils::conversion::toGeomMsgPt(p); - converted_centerline.push_back(converted_p); - } - const double lat_dist = - std::fabs(autoware::motion_utils::calcLateralOffset(converted_centerline, obj_point)); - - // Compute Chi-squared distributed (Equation (8) in the paper) - const double sigma_d = sigma_lateral_offset_; // Standard Deviation for lateral position - const double sigma_yaw = M_PI * sigma_yaw_angle_deg_ / 180.0; // Standard Deviation for yaw angle - const Eigen::Vector2d delta(lat_dist, abs_norm_delta_yaw); - const Eigen::Matrix2d P_inv = - (Eigen::Matrix2d() << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw)) - .finished(); - const double MINIMUM_DISTANCE = 1e-6; - const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); - - return static_cast(1.0 / dist); + return utils::getCurrentLanelets( + object, lanelet_map_ptr_, road_users_history_, dist_threshold_for_searching_lanelet_, + delta_yaw_threshold_for_searching_lanelet_, sigma_lateral_offset_, sigma_yaw_angle_deg_); } void MapBasedPredictionNode::updateRoadUsersHistory( @@ -1737,7 +1531,9 @@ std::optional MapBasedPredictionNode::getPredictionForVehicleOb updateObjectData(object); // Get Closest Lanelet - const auto current_lanelets = getCurrentLanelets(object); + const auto current_lanelets = utils::getCurrentLanelets( + object, lanelet_map_ptr_, road_users_history_, dist_threshold_for_searching_lanelet_, + delta_yaw_threshold_for_searching_lanelet_, sigma_lateral_offset_, sigma_yaw_angle_deg_); // Update Objects History updateRoadUsersHistory(header, object, current_lanelets); @@ -1975,40 +1771,6 @@ std::optional MapBasedPredictionNode::searchProperStartingRefPathIndex( return is_position_found ? opt_index : std::nullopt; } -bool MapBasedPredictionNode::isDuplicated( - const std::pair & target_lanelet, - const LaneletsData & lanelets_data) -{ - const double CLOSE_LANELET_THRESHOLD = 0.1; - for (const auto & lanelet_data : lanelets_data) { - const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); - const auto lanelet_end_p = lanelet_data.lanelet.centerline2d().back(); - const double dist = std::hypot( - target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); - if (dist < CLOSE_LANELET_THRESHOLD) { - return true; - } - } - - return false; -} - -bool MapBasedPredictionNode::isDuplicated( - const PredictedPath & predicted_path, const std::vector & predicted_paths) -{ - const double CLOSE_PATH_THRESHOLD = 0.1; - for (const auto & prev_predicted_path : predicted_paths) { - const auto prev_path_end = prev_predicted_path.path.back().position; - const auto current_path_end = predicted_path.path.back().position; - const double dist = autoware::universe_utils::calcDistance2d(prev_path_end, current_path_end); - if (dist < CLOSE_PATH_THRESHOLD) { - return true; - } - } - - return false; -} - } // namespace autoware::map_based_prediction #include diff --git a/perception/autoware_map_based_prediction/src/utils.cpp b/perception/autoware_map_based_prediction/src/utils.cpp index c9ebd9667659a..8c4a25a793bf7 100644 --- a/perception/autoware_map_based_prediction/src/utils.cpp +++ b/perception/autoware_map_based_prediction/src/utils.cpp @@ -14,6 +14,14 @@ #include "map_based_prediction/utils.hpp" +#include +#include +#include +#include + +#include +#include + namespace autoware::map_based_prediction { namespace utils @@ -218,5 +226,266 @@ PredictedObject convertToPredictedObject(const TrackedObject & tracked_object) return predicted_object; } +double calculateLocalLikelihood( + const lanelet::Lanelet & current_lanelet, const TrackedObject & object, + const double sigma_lateral_offset, const double sigma_yaw_angle_deg) +{ + const auto & obj_point = object.kinematics.pose_with_covariance.pose.position; + + // compute yaw difference between the object and lane + const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle(current_lanelet, obj_point); + const double delta_yaw = obj_yaw - lane_yaw; + const double abs_norm_delta_yaw = std::fabs(autoware::universe_utils::normalizeRadian(delta_yaw)); + + // compute lateral distance + const auto centerline = current_lanelet.centerline(); + std::vector converted_centerline; + for (const auto & p : centerline) { + const auto converted_p = lanelet::utils::conversion::toGeomMsgPt(p); + converted_centerline.push_back(converted_p); + } + const double lat_dist = + std::fabs(autoware::motion_utils::calcLateralOffset(converted_centerline, obj_point)); + + // Compute Chi-squared distributed (Equation (8) in the paper) + const double sigma_d = sigma_lateral_offset; // Standard Deviation for lateral position + const double sigma_yaw = M_PI * sigma_yaw_angle_deg / 180.0; // Standard Deviation for yaw angle + const Eigen::Vector2d delta(lat_dist, abs_norm_delta_yaw); + const Eigen::Matrix2d P_inv = + (Eigen::Matrix2d() << 1.0 / (sigma_d * sigma_d), 0.0, 0.0, 1.0 / (sigma_yaw * sigma_yaw)) + .finished(); + const double MINIMUM_DISTANCE = 1e-6; + const double dist = std::max(delta.dot(P_inv * delta), MINIMUM_DISTANCE); + + return 1.0 / dist; +} + +bool isDuplicated( + const std::pair & target_lanelet, + const LaneletsData & lanelets_data) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet_data : lanelets_data) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet_data.lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +bool isDuplicated( + const PredictedPath & predicted_path, const std::vector & predicted_paths) +{ + const double CLOSE_PATH_THRESHOLD = 0.1; + for (const auto & prev_predicted_path : predicted_paths) { + const auto prev_path_end = prev_predicted_path.path.back().position; + const auto current_path_end = predicted_path.path.back().position; + const double dist = autoware::universe_utils::calcDistance2d(prev_path_end, current_path_end); + if (dist < CLOSE_PATH_THRESHOLD) { + return true; + } + } + + return false; +} + +bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const std::unordered_map> & road_users_history, + const double dist_threshold_for_searching_lanelet, + const double delta_yaw_threshold_for_searching_lanelet) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // If the object is in the objects history, we check if the target lanelet is + // inside the current lanelets id or following lanelets + const std::string object_id = autoware::universe_utils::toHexString(object.object_id); + if (road_users_history.count(object_id) != 0) { + const std::vector & possible_lanelet = + road_users_history.at(object_id).back().future_possible_lanelets; + + bool not_in_possible_lanelet = + std::find(possible_lanelet.begin(), possible_lanelet.end(), lanelet.second) == + possible_lanelet.end(); + if (!possible_lanelet.empty() && not_in_possible_lanelet) { + return false; + } + } + + // Step2. Calculate the angle difference between the lane angle and obstacle angle + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step3. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const bool is_yaw_reversed = + M_PI - delta_yaw_threshold_for_searching_lanelet < abs_norm_delta && object_vel < 0.0; + if (dist_threshold_for_searching_lanelet < lanelet.first) { + return false; + } + if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet < abs_norm_delta) { + return false; + } + + return true; +} + +// NOTE: These two functions are copied from the route_handler package. +lanelet::Lanelets getRightOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.rightBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.leftBound().id() == lanelet.rightBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +lanelet::Lanelets getLeftOppositeLanelets( + const std::shared_ptr & lanelet_map_ptr, + const lanelet::ConstLanelet & lanelet) +{ + const auto opposite_candidate_lanelets = + lanelet_map_ptr->laneletLayer.findUsages(lanelet.leftBound().invert()); + + lanelet::Lanelets opposite_lanelets; + for (const auto & candidate_lanelet : opposite_candidate_lanelets) { + if (candidate_lanelet.rightBound().id() == lanelet.leftBound().id()) { + continue; + } + + opposite_lanelets.push_back(candidate_lanelet); + } + + return opposite_lanelets; +} + +LaneletsData getCurrentLanelets( + const TrackedObject & object, lanelet::LaneletMapPtr lanelet_map_ptr, + const std::unordered_map> & road_users_history, + const double dist_threshold_for_searching_lanelet, + const double delta_yaw_threshold_for_searching_lanelet, const double sigma_lateral_offset, + const double sigma_yaw_angle_deg) +{ + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + { // Step 1. Search same directional lanelets + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + LaneletsData object_lanelets; + std::optional> closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the object lanelet + if ( + !checkCloseLaneletCondition( + lanelet, object, road_users_history, dist_threshold_for_searching_lanelet, + delta_yaw_threshold_for_searching_lanelet) || + isDuplicated(lanelet, object_lanelets)) { + continue; + } + + // Memorize closest lanelet + // NOTE: The object may be outside the lanelet. + if (!closest_lanelet || lanelet.first < closest_lanelet->first) { + closest_lanelet = lanelet; + } + + // Check if the obstacle is inside of this lanelet + constexpr double epsilon = 1e-3; + if (lanelet.first < epsilon) { + const auto object_lanelet = LaneletData{ + lanelet.second, utils::calculateLocalLikelihood( + lanelet.second, object, sigma_lateral_offset, sigma_yaw_angle_deg)}; + object_lanelets.push_back(object_lanelet); + } + } + + if (!object_lanelets.empty()) { + return object_lanelets; + } + if (closest_lanelet) { + return LaneletsData{LaneletData{ + closest_lanelet->second, + utils::calculateLocalLikelihood( + closest_lanelet->second, object, sigma_lateral_offset, sigma_yaw_angle_deg)}}; + } + } + + { // Step 2. Search opposite directional lanelets + // Get opposite lanelets and calculate distance to search point. + std::vector> surrounding_opposite_lanelets; + for (const auto & surrounding_lanelet : surrounding_lanelets) { + for (const auto & left_opposite_lanelet : + getLeftOppositeLanelets(lanelet_map_ptr, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(left_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, left_opposite_lanelet)); + } + for (const auto & right_opposite_lanelet : + getRightOppositeLanelets(lanelet_map_ptr, surrounding_lanelet.second)) { + const double distance = lanelet::geometry::distance2d(right_opposite_lanelet, search_point); + surrounding_opposite_lanelets.push_back(std::make_pair(distance, right_opposite_lanelet)); + } + } + + std::optional> opposite_closest_lanelet{std::nullopt}; + for (const auto & lanelet : surrounding_opposite_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets + // except for distance checking + if (!checkCloseLaneletCondition( + lanelet, object, road_users_history, dist_threshold_for_searching_lanelet, + delta_yaw_threshold_for_searching_lanelet)) { + continue; + } + + // Memorize closest lanelet + if (!opposite_closest_lanelet || lanelet.first < opposite_closest_lanelet->first) { + opposite_closest_lanelet = lanelet; + } + } + if (opposite_closest_lanelet) { + return LaneletsData{LaneletData{ + opposite_closest_lanelet->second, + utils::calculateLocalLikelihood( + opposite_closest_lanelet->second, object, sigma_lateral_offset, sigma_yaw_angle_deg)}}; + } + } + + return LaneletsData{}; +} + } // namespace utils } // namespace autoware::map_based_prediction