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

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 @@
     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);

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -922,6 +922,7 @@
   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,93 +2374,98 @@
 }
 
 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;
 
     // Insert Positions. Note that we start inserting points from previous lanelet
     if (!path.empty()) {
       lanelet::ConstLanelets prev_lanelets = routing_graph_ptr_->previous(path.front());
       if (!prev_lanelets.empty()) {
         lanelet::ConstLanelet prev_lanelet = prev_lanelets.front();
         bool init_flag = true;
         geometry_msgs::msg::Pose prev_p;
         for (const auto & lanelet_p : prev_lanelet.centerline()) {
           geometry_msgs::msg::Pose current_p;
           current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p);
           if (init_flag) {
             init_flag = false;
             prev_p = current_p;
             continue;
           }
 
           const double lane_yaw = std::atan2(
             current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
           const double sin_yaw_half = std::sin(lane_yaw / 2.0);
           const double cos_yaw_half = std::cos(lane_yaw / 2.0);
           current_p.orientation.x = 0.0;
           current_p.orientation.y = 0.0;
           current_p.orientation.z = sin_yaw_half;
           current_p.orientation.w = cos_yaw_half;
 
           converted_path.push_back(current_p);
           prev_p = current_p;
         }
       }
     }
 
     for (const auto & lanelet : path) {
       bool init_flag = true;
       geometry_msgs::msg::Pose prev_p;
       for (const auto & lanelet_p : lanelet.centerline()) {
         geometry_msgs::msg::Pose current_p;
         current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p);
         if (init_flag) {
           init_flag = false;
           prev_p = current_p;
           continue;
         }
 
         // Prevent from inserting same points
         if (!converted_path.empty()) {
           const auto last_p = converted_path.back();
           const double tmp_dist = autoware::universe_utils::calcDistance2d(last_p, current_p);
           if (tmp_dist < 1e-6) {
             prev_p = current_p;
             continue;
           }
         }
 
         const double lane_yaw = std::atan2(
           current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
         const double sin_yaw_half = std::sin(lane_yaw / 2.0);
         const double cos_yaw_half = std::cos(lane_yaw / 2.0);
         current_p.orientation.x = 0.0;
         current_p.orientation.y = 0.0;
         current_p.orientation.z = sin_yaw_half;
         current_p.orientation.w = cos_yaw_half;
 
         converted_path.push_back(current_p);
         prev_p = current_p;
       }
     }
 
     // Resample Path
     const bool use_akima_spline_for_xy = true;
     const bool use_lerp_for_z = true;
     // the options use_akima_spline_for_xy and use_lerp_for_z are set to true
     // but the implementation of use_akima_spline_for_xy in resamplePoseVector and
     // resamplePointVector is opposite to the options so the options are set to true to use linear
     // interpolation for xy
     const auto resampled_converted_path = autoware::motion_utils::resamplePoseVector(
       converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z);
     converted_paths.push_back(resampled_converted_path);
   }
 
+  lru_cache_of_convert_path_type_.put(paths, converted_paths);
   return converted_paths;
 }