From 93b9a5b75f7127dc285315c977076568ec36a5f9 Mon Sep 17 00:00:00 2001
From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com>
Date: Wed, 14 Aug 2024 17:32:39 +0900
Subject: [PATCH] perf(autoware_map_based_prediction): speed up map based
 prediction by using lru cache in convertPathType (#8461)

feat(autoware_map_based_prediction): speed up map based prediction by using lru cache in convertPathType

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
---
 .../map_based_prediction_node.hpp             | 28 ++++++++++++++++++-
 .../src/map_based_prediction_node.cpp         |  8 +++++-
 2 files changed, 34 insertions(+), 2 deletions(-)

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 <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
+#include <autoware/universe_utils/system/lru_cache.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -41,6 +42,7 @@
 #include <lanelet2_core/Forward.h>
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/Forward.h>
+#include <lanelet2_routing/LaneletPath.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
 #include <algorithm>
@@ -54,6 +56,27 @@
 #include <utility>
 #include <vector>
 
+namespace std
+{
+template <>
+struct hash<lanelet::routing::LaneletPaths>
+{
+  // 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<int64_t>{}(lanelet.id()) + 0x9e3779b9 + (seed << 6U) + (seed >> 2U);
+      }
+      // Add a separator between paths
+      seed ^= hash<int>{}(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<PredictedRefPath> & reference_paths,
     const double speed_limit = 0.0);
-  std::vector<PosePath> convertPathType(const lanelet::routing::LaneletPaths & paths);
+
+  mutable universe_utils::LRUCache<lanelet::routing::LaneletPaths, std::vector<PosePath>>
+    lru_cache_of_convert_path_type_{1000};
+  std::vector<PosePath> 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::LaneletMap>();
   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<PosePath> 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<PosePath> converted_paths;
   for (const auto & path : paths) {
     PosePath converted_path;
@@ -2460,6 +2465,7 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
     converted_paths.push_back(resampled_converted_path);
   }
 
+  lru_cache_of_convert_path_type_.put(paths, converted_paths);
   return converted_paths;
 }