diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 8440fd25bcec5..725d9ee5de687 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include @@ -42,6 +44,28 @@ #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 map_based_prediction { struct LateralKinematicsToLanelet @@ -223,6 +247,10 @@ class MapBasedPredictionNode : public rclcpp::Node const Maneuver & maneuver, std::vector & reference_paths); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); + mutable tier4_autoware_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/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index d23d57ddd9301..66fc5652fabe3 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -828,6 +828,7 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::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_INFO(get_logger(), "[Map Based Prediction]: Map is loaded"); const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -1994,6 +1995,10 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( std::vector MapBasedPredictionNode::convertPathType( const lanelet::routing::LaneletPaths & paths) { + 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; @@ -2077,6 +2082,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; }