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 b22609100bee1..1f13391ae442a 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,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include #include @@ -54,6 +56,27 @@ #include #include +namespace std +{ +template <> +struct hash +{ + // 0x9e3779b9 is a magic number. See + // https://stackoverflow.com/questions/4948780/magic-number-in-boosthash-combine + size_t operator()(const lanelet::routing::LaneletPaths & paths) const + { + size_t seed = 0; + for (const auto & path : paths) { + for (const auto & lanelet : path) { + seed ^= hash{}(lanelet.id()) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + // Add a separator between paths + seed ^= hash{}(0) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U); + } + return seed; + } +}; +} // namespace std namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet @@ -291,7 +314,10 @@ class MapBasedPredictionNode : public rclcpp::Node const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths, const double speed_limit = 0.0); - std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + + mutable universe_utils::LRUCache> + lru_cache_of_convert_path_type_{1000}; + std::vector convertPathType(const lanelet::routing::LaneletPaths & paths) const; void updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); 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 473a0335b1def..9c9169c1bb495 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 @@ -922,6 +922,7 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lru_cache_of_convert_path_type_.clear(); // clear cache RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -2373,10 +2374,14 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( } std::vector MapBasedPredictionNode::convertPathType( - const lanelet::routing::LaneletPaths & paths) + const lanelet::routing::LaneletPaths & paths) const { autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_); + if (lru_cache_of_convert_path_type_.contains(paths)) { + return *lru_cache_of_convert_path_type_.get(paths); + } + std::vector converted_paths; for (const auto & path : paths) { PosePath converted_path; @@ -2460,6 +2465,7 @@ std::vector MapBasedPredictionNode::convertPathType( converted_paths.push_back(resampled_converted_path); } + lru_cache_of_convert_path_type_.put(paths, converted_paths); return converted_paths; }