From dd1493d240f10413d0be04113397af04678652e9 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 14:31:49 +0900
Subject: [PATCH 01/16] perf: PR 7237

https://github.com/autowarefoundation/autoware.universe/pull/7237
---
 .../src/map_based_prediction_node.cpp                | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

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 85535df4babfe..65f8ca591bf2b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1031,10 +1031,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
 {
-  const lanelet::ConstLineStrings3d & all_fences =
-    lanelet::utils::query::getAllFences(lanelet_map_ptr_);
-  for (const auto & fence_line : all_fences) {
-    if (doesPathCrossFence(predicted_path, fence_line)) {
+  lanelet::BasicLineString2d predicted_path_ls;
+  for (const auto & p : predicted_path.path)
+    predicted_path_ls.emplace_back(p.position.x, p.position.y);
+  const auto candidates =
+    lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
+  for (const auto & candidate : candidates) {
+    const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) {
       return true;
     }
   }

From 0b0893ccd2bf789965c46a84fbb7ddcce1417433 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 14:55:32 +0900
Subject: [PATCH 02/16] perf RP8406

https://github.com/autowarefoundation/autoware.universe/pull/8406
---
 .../map_based_prediction_node.hpp                 |  4 ++++
 .../src/map_based_prediction_node.cpp             | 15 ++++++++++++---
 2 files changed, 16 insertions(+), 3 deletions(-)

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 bdd9ad89bc025..8440fd25bcec5 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
@@ -31,6 +31,7 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/Forward.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
@@ -132,6 +133,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Crosswalk Entry Points
   lanelet::ConstLanelets crosswalks_;
 
+  // Fences
+  lanelet::LaneletMapUPtr fence_layer_{nullptr};
+
   // Parameters
   bool enable_delay_compensation_;
   double prediction_time_horizon_;
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 65f8ca591bf2b..5f9492559cb5c 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -835,6 +835,16 @@ void MapBasedPredictionNode::mapCallback(const HADMapBin::ConstSharedPtr msg)
   const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets);
   crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end());
   crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end());
+
+  lanelet::LineStrings3d fences;
+  for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) {
+    if (const std::string type = linestring.attributeOr(lanelet::AttributeName::Type, "none");
+        type == "fence") {
+      fences.push_back(lanelet::LineString3d(
+        std::const_pointer_cast<lanelet::LineStringData>(linestring.constData())));
+    }
+  }
+  fence_layer_ = lanelet::utils::createMap(fences);
 }
 
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
@@ -1035,10 +1045,9 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict
   for (const auto & p : predicted_path.path)
     predicted_path_ls.emplace_back(p.position.x, p.position.y);
   const auto candidates =
-    lanelet_map_ptr_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
+    fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
   for (const auto & candidate : candidates) {
-    const std::string type = candidate.attributeOr(lanelet::AttributeName::Type, "none");
-    if (type == "fence" && doesPathCrossFence(predicted_path, candidate)) {
+    if (doesPathCrossFence(predicted_path, candidate)) {
       return true;
     }
   }

From b75ca968e3be8322e606e53fc83860079faca73a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 14:58:56 +0900
Subject: [PATCH 03/16] perf PR 8416

---
 .../src/path_generator.cpp                    | 22 +++++++++----------
 1 file changed, 10 insertions(+), 12 deletions(-)

diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp
index 5cb7e186b7cc4..a7dda00dc4f21 100644
--- a/perception/map_based_prediction/src/path_generator.cpp
+++ b/perception/map_based_prediction/src/path_generator.cpp
@@ -14,7 +14,7 @@
 
 #include "map_based_prediction/path_generator.hpp"
 
-#include <interpolation/spline_interpolation.hpp>
+#include <interpolation/linear_interpolation.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
@@ -327,31 +327,29 @@ PosePath PathGenerator::interpolateReferencePath(
     std::reverse(resampled_s.begin(), resampled_s.end());
   }
 
-  // Spline Interpolation
-  std::vector<double> spline_ref_path_x =
-    interpolation::spline(base_path_s, base_path_x, resampled_s);
-  std::vector<double> spline_ref_path_y =
-    interpolation::spline(base_path_s, base_path_y, resampled_s);
-  std::vector<double> spline_ref_path_z =
-    interpolation::spline(base_path_s, base_path_z, resampled_s);
+  // Lerp Interpolation
+  std::vector<double> lerp_ref_path_x = interpolation::lerp(base_path_s, base_path_x, resampled_s);
+  std::vector<double> lerp_ref_path_y = interpolation::lerp(base_path_s, base_path_y, resampled_s);
+  std::vector<double> lerp_ref_path_z = interpolation::lerp(base_path_s, base_path_z, resampled_s);
 
   interpolated_path.resize(interpolate_num);
   for (size_t i = 0; i < interpolate_num - 1; ++i) {
     geometry_msgs::msg::Pose interpolated_pose;
     const auto current_point =
-      tier4_autoware_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0);
+      tier4_autoware_utils::createPoint(lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), 0.0);
     const auto next_point = tier4_autoware_utils::createPoint(
-      spline_ref_path_x.at(i + 1), spline_ref_path_y.at(i + 1), 0.0);
+      lerp_ref_path_x.at(i + 1), lerp_ref_path_y.at(i + 1), 0.0);
     const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point);
     interpolated_pose.position = tier4_autoware_utils::createPoint(
-      spline_ref_path_x.at(i), spline_ref_path_y.at(i), spline_ref_path_z.at(i));
+      lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), lerp_ref_path_z.at(i));
     interpolated_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
     interpolated_path.at(i) = interpolated_pose;
   }
   interpolated_path.back().position = tier4_autoware_utils::createPoint(
-    spline_ref_path_x.back(), spline_ref_path_y.back(), spline_ref_path_z.back());
+    lerp_ref_path_x.back(), lerp_ref_path_y.back(), lerp_ref_path_z.back());
   interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation;
 
+
   return interpolated_path;
 }
 

From 36fb43e558c3cabef35b7f871e01db79a64f1c68 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 15:03:35 +0900
Subject: [PATCH 04/16] perf PR 8427

---
 .../src/map_based_prediction_node.cpp         | 26 ++++++++++++++++---
 1 file changed, 22 insertions(+), 4 deletions(-)

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 5f9492559cb5c..14937a927882c 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -2021,7 +2021,13 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
 
           const double lane_yaw = std::atan2(
             current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
-          current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
+          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;
         }
@@ -2052,15 +2058,27 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
 
         const double lane_yaw = std::atan2(
           current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
-        current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw);
+        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 auto resampled_converted_path =
-      motion_utils::resamplePoseVector(converted_path, reference_path_resolution_);
+    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 = motion_utils::resamplePoseVector(
+      converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z);
     converted_paths.push_back(resampled_converted_path);
   }
 

From 480856351ea13051690c0edae864b462417655a4 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 15:07:42 +0900
Subject: [PATCH 05/16] perf PR 8413

---
 .../src/map_based_prediction_node.cpp         | 31 ++++++++-----------
 1 file changed, 13 insertions(+), 18 deletions(-)

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 14937a927882c..d23d57ddd9301 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -855,23 +855,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
     return;
   }
 
-  auto world2map_transform = transform_listener_.getTransform(
-    "map",                        // target
-    in_objects->header.frame_id,  // src
-    in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
-  auto map2world_transform = transform_listener_.getTransform(
-    in_objects->header.frame_id,  // target
-    "map",                        // src
-    in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
-  auto debug_map2lidar_transform = transform_listener_.getTransform(
-    "base_link",  // target
-    "map",        // src
-    rclcpp::Time(), rclcpp::Duration::from_seconds(0.1));
-
-  if (!world2map_transform || !map2world_transform || !debug_map2lidar_transform) {
-    return;
-  }
-
   // Remove old objects information in object history
   const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
   removeOldObjectsHistory(objects_detected_time);
@@ -884,12 +867,24 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   // result debug
   visualization_msgs::msg::MarkerArray debug_markers;
 
+  // get world to map transform
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform;
+
+  bool is_object_not_in_map_frame = in_objects->header.frame_id != "map";
+  if (is_object_not_in_map_frame) {
+    world2map_transform = transform_listener_.getTransform(
+      "map",                        // target
+      in_objects->header.frame_id,  // src
+      in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
+    if (!world2map_transform) return;
+  }
+
   for (const auto & object : in_objects->objects) {
     std::string object_id = tier4_autoware_utils::toHexString(object.object_id);
     TrackedObject transformed_object = object;
 
     // transform object frame if it's based on map frame
-    if (in_objects->header.frame_id != "map") {
+    if (is_object_not_in_map_frame) {
       geometry_msgs::msg::PoseStamped pose_in_map;
       geometry_msgs::msg::PoseStamped pose_orig;
       pose_orig.pose = object.kinematics.pose_with_covariance.pose;

From 3f18f0d8ee7b80119bfdb8e8d875ccd228021183 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 15:40:07 +0900
Subject: [PATCH 06/16] tool PR 8456

---
 .../tier4_autoware_utils/system/lru_cache.hpp | 142 ++++++++++++++++++
 .../test/src/system/test_lru_cache.cpp        |  78 ++++++++++
 2 files changed, 220 insertions(+)
 create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp
 create mode 100644 common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp
new file mode 100644
index 0000000000000..3734f8ecd84ed
--- /dev/null
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/lru_cache.hpp
@@ -0,0 +1,142 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_
+#define TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_
+
+#include <cstddef>
+#include <list>
+#include <optional>
+#include <unordered_map>
+#include <utility>
+
+namespace tier4_autoware_utils
+{
+
+/**
+ * @brief A template class for LRU (Least Recently Used) Cache.
+ *
+ * This class implements a simple LRU cache using a combination of a list and a hash map.
+ *
+ * @tparam Key The type of keys.
+ * @tparam Value The type of values.
+ * @tparam Map The type of underlying map, defaulted to std::unordered_map.
+ */
+template <typename Key, typename Value, template <typename...> class Map = std::unordered_map>
+class LRUCache
+{
+private:
+  size_t capacity_;                              ///< The maximum capacity of the cache.
+  std::list<std::pair<Key, Value>> cache_list_;  ///< List to maintain the order of elements.
+  Map<Key, typename std::list<std::pair<Key, Value>>::iterator>
+    cache_map_;  ///< Map for fast access to elements.
+
+public:
+  /**
+   * @brief Construct a new LRUCache object.
+   *
+   * @param size The capacity of the cache.
+   */
+  explicit LRUCache(size_t size) : capacity_(size) {}
+
+  /**
+   * @brief Get the capacity of the cache.
+   *
+   * @return The capacity of the cache.
+   */
+  [[nodiscard]] size_t capacity() const { return capacity_; }
+
+  /**
+   * @brief Insert a key-value pair into the cache.
+   *
+   * If the key already exists, its value is updated and it is moved to the front.
+   * If the cache exceeds its capacity, the least recently used element is removed.
+   *
+   * @param key The key to insert.
+   * @param value The value to insert.
+   */
+  void put(const Key & key, const Value & value)
+  {
+    auto it = cache_map_.find(key);
+    if (it != cache_map_.end()) {
+      cache_list_.erase(it->second);
+    }
+    cache_list_.push_front({key, value});
+    cache_map_[key] = cache_list_.begin();
+
+    if (cache_map_.size() > capacity_) {
+      auto last = cache_list_.back();
+      cache_map_.erase(last.first);
+      cache_list_.pop_back();
+    }
+  }
+
+  /**
+   * @brief Retrieve a value from the cache.
+   *
+   * If the key does not exist in the cache, std::nullopt is returned.
+   * If the key exists, the value is returned and the element is moved to the front.
+   *
+   * @param key The key to retrieve.
+   * @return The value associated with the key, or std::nullopt if the key does not exist.
+   */
+  std::optional<Value> get(const Key & key)
+  {
+    auto it = cache_map_.find(key);
+    if (it == cache_map_.end()) {
+      return std::nullopt;
+    }
+    cache_list_.splice(cache_list_.begin(), cache_list_, it->second);
+    return it->second->second;
+  }
+
+  /**
+   * @brief Clear the cache.
+   *
+   * This removes all elements from the cache.
+   */
+  void clear()
+  {
+    cache_list_.clear();
+    cache_map_.clear();
+  }
+
+  /**
+   * @brief Get the current size of the cache.
+   *
+   * @return The number of elements in the cache.
+   */
+  [[nodiscard]] size_t size() const { return cache_map_.size(); }
+
+  /**
+   * @brief Check if the cache is empty.
+   *
+   * @return True if the cache is empty, false otherwise.
+   */
+  [[nodiscard]] bool empty() const { return cache_map_.empty(); }
+
+  /**
+   * @brief Check if a key exists in the cache.
+   *
+   * @param key The key to check.
+   * @return True if the key exists, false otherwise.
+   */
+  [[nodiscard]] bool contains(const Key & key) const
+  {
+    return cache_map_.find(key) != cache_map_.end();
+  }
+};
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__SYSTEM__LRU_CACHE_HPP_
diff --git a/common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp b/common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp
new file mode 100644
index 0000000000000..f1cd2414f50fe
--- /dev/null
+++ b/common/tier4_autoware_utils/test/src/system/test_lru_cache.cpp
@@ -0,0 +1,78 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "tier4_autoware_utils/system/lru_cache.hpp"
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <utility>
+
+using tier4_autoware_utils::LRUCache;
+
+// Fibonacci calculation with LRU cache
+int64_t fibonacci_with_cache(int n, LRUCache<int, int64_t> * cache)
+{
+  if (n <= 1) return n;
+
+  if (cache->contains(n)) {
+    return *cache->get(n);
+  }
+  int64_t result = fibonacci_with_cache(n - 1, cache) + fibonacci_with_cache(n - 2, cache);
+  cache->put(n, result);
+  return result;
+}
+
+// Fibonacci calculation without cache
+int64_t fibonacci_no_cache(int n)
+{
+  if (n <= 1) return n;
+  return fibonacci_no_cache(n - 1) + fibonacci_no_cache(n - 2);
+}
+
+// Helper function to measure execution time
+template <typename Func, typename... Args>
+std::pair<int64_t, decltype(std::declval<Func>()(std::declval<Args>()...))> measure_time(
+  Func func, Args &&... args)
+{
+  auto start = std::chrono::high_resolution_clock::now();
+  auto result = func(std::forward<Args>(args)...);
+  auto end = std::chrono::high_resolution_clock::now();
+  return {std::chrono::duration_cast<std::chrono::microseconds>(end - start).count(), result};
+}
+
+// Test case to verify Fibonacci calculation results with and without cache
+TEST(FibonacciLRUCacheTest, CompareResultsAndPerformance)
+{
+  const int max_n = 40;                 // Test range
+  LRUCache<int, int64_t> cache(max_n);  // Cache with capacity set to max_n
+
+  for (int i = 0; i <= max_n; ++i) {
+    // Measure time for performance comparison
+    auto [time_with_cache, result_with_cache] =
+      measure_time([i, &cache]() { return fibonacci_with_cache(i, &cache); });
+    auto [time_without_cache, result_without_cache] =
+      measure_time([i]() { return fibonacci_no_cache(i); });
+
+    EXPECT_EQ(result_with_cache, result_without_cache) << "Mismatch at n = " << i;
+
+    // Print the calculation time
+    std::cout << "n = " << i << ": With Cache = " << time_with_cache
+              << " μs, Without Cache = " << time_without_cache << " μs\n";
+
+    // // Clear the cache after each iteration to ensure fair comparison
+    cache.clear();
+  }
+}

From 36b0cb6c39f190cc9a749ea7585e618249a0689a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 15:42:54 +0900
Subject: [PATCH 07/16] perf PR 8461

---
 .../map_based_prediction_node.hpp             | 28 +++++++++++++++++++
 .../src/map_based_prediction_node.cpp         |  6 ++++
 2 files changed, 34 insertions(+)

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 <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <tier4_autoware_utils/system/lru_cache.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
@@ -33,6 +34,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 <deque>
@@ -42,6 +44,28 @@
 #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 map_based_prediction
 {
 struct LateralKinematicsToLanelet
@@ -223,6 +247,10 @@ class MapBasedPredictionNode : public rclcpp::Node
     const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths);
   std::vector<PosePath> convertPathType(const lanelet::routing::LaneletPaths & paths);
 
+  mutable tier4_autoware_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/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::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_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<PosePath> 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<PosePath> converted_paths;
   for (const auto & path : paths) {
     PosePath converted_path;
@@ -2077,6 +2082,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;
 }
 

From f6fa013cf1d63971a906e62b3c0b2d974a23c58e Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 15:55:35 +0900
Subject: [PATCH 08/16] perf PR 8388

---
 .../src/map_based_prediction_node.cpp         | 68 +++++++++++--------
 1 file changed, 39 insertions(+), 29 deletions(-)

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 66fc5652fabe3..2613d164f8c8b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -415,9 +415,9 @@ bool withinRoadLanelet(
 }
 
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
-  const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk,
-  const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr,
-  const double time_horizon, const double min_object_vel)
+  const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
+  const lanelet::ConstLanelets & external_surrounding_crosswalks,
+  const CrosswalkEdgePoints & edge_points, const double time_horizon, const double min_object_vel)
 {
   using Point = boost::geometry::model::d2::point_xy<double>;
 
@@ -425,11 +425,6 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
   const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z;
 
-  lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y);
-  if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) {
-    return {};
-  }
-
   const auto & p1 = edge_points.front_center_point;
   const auto & p2 = edge_points.back_center_point;
 
@@ -446,17 +441,12 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto is_stop_object = estimated_velocity < stop_velocity_th;
   const auto velocity = std::max(min_object_vel, estimated_velocity);
-  const auto surrounding_lanelets = lanelet::geometry::findNearest(
-    lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity);
 
-  const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) {
-    const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) {
-      for (const auto & lanelet : surrounding_lanelets) {
-        const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
-        if (
-          (attr.value() == lanelet::AttributeValueString::Crosswalk ||
-           attr.value() == lanelet::AttributeValueString::Walkway) &&
-          boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) {
+  const auto isAcrossAnyRoad = [&surrounding_lanelets, &external_surrounding_crosswalks](
+                                 const Point & p_src, const Point & p_dst) {
+    const auto withinAnyCrosswalk = [&external_surrounding_crosswalks](const Point & p) {
+      for (const auto & crosswalk : external_surrounding_crosswalks) {
+        if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) {
           return true;
         }
       }
@@ -475,14 +465,13 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
     std::vector<Point> points_of_intersect;
     const boost::geometry::model::linestring<Point> line{p_src, p_dst};
     for (const auto & lanelet : surrounding_lanelets) {
-      const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
+      const lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (attr.value() != lanelet::AttributeValueString::Road) {
         continue;
       }
 
       std::vector<Point> tmp_intersects;
-      boost::geometry::intersection(
-        line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects);
+      boost::geometry::intersection(line, lanelet.polygon2d().basicPolygon(), tmp_intersects);
       for (const auto & p : tmp_intersects) {
         if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) {
           continue;
@@ -1090,10 +1079,29 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   }
 
   boost::optional<lanelet::ConstLanelet> crossing_crosswalk{boost::none};
-  for (const auto & crosswalk : crosswalks_) {
-    if (withinLanelet(object, crosswalk)) {
-      crossing_crosswalk = crosswalk;
-      break;
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
+  const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
+  const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
+  // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
+  // implementation has been wrong.
+  const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
+    lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
+    prediction_time_horizon_ * velocity);
+  lanelet::ConstLanelets surrounding_lanelets;
+  lanelet::ConstLanelets external_surrounding_crosswalks;
+  for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
+    surrounding_lanelets.push_back(lanelet);
+    const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype);
+    if (
+      attr.value() == lanelet::AttributeValueString::Crosswalk ||
+      attr.value() == lanelet::AttributeValueString::Walkway) {
+      const auto & crosswalk = lanelet;
+      if (withinLanelet(object, crosswalk)) {
+        crossing_crosswalk = crosswalk;
+      } else {
+        external_surrounding_crosswalks.push_back(crosswalk);
+      }
     }
   }
 
@@ -1153,8 +1161,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       }
     }
   }
-  // try to find the edge points for all crosswalks and generate path to the crosswalk edge
-  for (const auto & crosswalk : crosswalks_) {
+
+  // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk
+  // edge
+  for (const auto & crosswalk : external_surrounding_crosswalks) {
     const auto edge_points = getCrosswalkEdgePoints(crosswalk);
 
     const auto reachable_first = hasPotentialToReach(
@@ -1171,8 +1181,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_,
-      min_crosswalk_user_velocity_);
+      object, surrounding_lanelets, external_surrounding_crosswalks, edge_points,
+      prediction_time_horizon_, min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;

From 1fe3ce5691fe97dabe71e74334de13a66c4e2bd3 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:01:53 +0900
Subject: [PATCH 09/16] perf PR 8467

---
 .../src/map_based_prediction_node.cpp         | 23 +++++++++++--------
 1 file changed, 13 insertions(+), 10 deletions(-)

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 2613d164f8c8b..6fc9f479c3d03 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -416,8 +416,8 @@ bool withinRoadLanelet(
 
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
-  const lanelet::ConstLanelets & external_surrounding_crosswalks,
-  const CrosswalkEdgePoints & edge_points, const double time_horizon, const double min_object_vel)
+  const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points,
+  const double time_horizon, const double min_object_vel)
 {
   using Point = boost::geometry::model::d2::point_xy<double>;
 
@@ -442,10 +442,10 @@ boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const auto is_stop_object = estimated_velocity < stop_velocity_th;
   const auto velocity = std::max(min_object_vel, estimated_velocity);
 
-  const auto isAcrossAnyRoad = [&surrounding_lanelets, &external_surrounding_crosswalks](
+  const auto isAcrossAnyRoad = [&surrounding_lanelets, &surrounding_crosswalks](
                                  const Point & p_src, const Point & p_dst) {
-    const auto withinAnyCrosswalk = [&external_surrounding_crosswalks](const Point & p) {
-      for (const auto & crosswalk : external_surrounding_crosswalks) {
+    const auto withinAnyCrosswalk = [&surrounding_crosswalks](const Point & p) {
+      for (const auto & crosswalk : surrounding_crosswalks) {
         if (boost::geometry::within(p, crosswalk.polygon2d().basicPolygon())) {
           return true;
         }
@@ -1089,7 +1089,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
     prediction_time_horizon_ * velocity);
   lanelet::ConstLanelets surrounding_lanelets;
-  lanelet::ConstLanelets external_surrounding_crosswalks;
+  lanelet::ConstLanelets surrounding_crosswalks;
   for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
     surrounding_lanelets.push_back(lanelet);
     const auto attr = lanelet.attribute(lanelet::AttributeName::Subtype);
@@ -1097,10 +1097,9 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       attr.value() == lanelet::AttributeValueString::Crosswalk ||
       attr.value() == lanelet::AttributeValueString::Walkway) {
       const auto & crosswalk = lanelet;
+      surrounding_crosswalks.push_back(crosswalk);
       if (withinLanelet(object, crosswalk)) {
         crossing_crosswalk = crosswalk;
-      } else {
-        external_surrounding_crosswalks.push_back(crosswalk);
       }
     }
   }
@@ -1164,7 +1163,11 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
   // try to find the edge points for other surrounding crosswalks and generate path to the crosswalk
   // edge
-  for (const auto & crosswalk : external_surrounding_crosswalks) {
+  for (const auto & crosswalk : surrounding_crosswalks) {
+    if (crossing_crosswalk && crossing_crosswalk.get() == crosswalk) {
+      continue;
+    }
+
     const auto edge_points = getCrosswalkEdgePoints(crosswalk);
 
     const auto reachable_first = hasPotentialToReach(
@@ -1181,7 +1184,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, surrounding_lanelets, external_surrounding_crosswalks, edge_points,
+      object, surrounding_lanelets, surrounding_crosswalks, edge_points,
       prediction_time_horizon_, min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {

From 41c35b0d4f9a9eef798a5c3d8be030502f3c44c0 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:06:18 +0900
Subject: [PATCH 10/16] perf PR 8471

---
 .../src/map_based_prediction_node.cpp         | 45 +++++++------------
 1 file changed, 16 insertions(+), 29 deletions(-)

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 6fc9f479c3d03..f740469a50f12 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -359,28 +359,6 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
                              back_center_point,  r_p_back,  l_p_back};
 }
 
-bool withinLanelet(
-  const TrackedObject & object, const lanelet::ConstLanelet & lanelet,
-  const bool use_yaw_information = false, const float yaw_threshold = 0.6)
-{
-  using Point = boost::geometry::model::d2::point_xy<double>;
-
-  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  const Point p_object{obj_pos.x, obj_pos.y};
-
-  auto polygon = lanelet.polygon2d().basicPolygon();
-  boost::geometry::correct(polygon);
-  bool with_in_polygon = boost::geometry::within(p_object, polygon);
-
-  if (!use_yaw_information) return with_in_polygon;
-
-  // use yaw angle to compare
-  const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet);
-  if (abs_yaw_diff < yaw_threshold) return with_in_polygon;
-
-  return false;
-}
-
 bool withinRoadLanelet(
   const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const bool use_yaw_information = false)
@@ -396,9 +374,12 @@ bool withinRoadLanelet(
   const auto surrounding_lanelets =
     lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
 
-  for (const auto & lanelet : surrounding_lanelets) {
-    if (lanelet.second.hasAttribute(lanelet::AttributeName::Subtype)) {
-      lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype);
+  for (const auto & lanelet_with_dist : surrounding_lanelets) {
+    const auto & dist = lanelet_with_dist.first;
+    const auto & lanelet = lanelet_with_dist.second;
+
+    if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
+      lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (
         attr.value() == lanelet::AttributeValueString::Crosswalk ||
         attr.value() == lanelet::AttributeValueString::Walkway) {
@@ -406,7 +387,13 @@ bool withinRoadLanelet(
       }
     }
 
-    if (withinLanelet(object, lanelet.second, use_yaw_information)) {
+    constexpr float yaw_threshold = 0.6;
+    bool within_lanelet = std::abs(dist) < 1e-5;
+    if (use_yaw_information) {
+      within_lanelet =
+        within_lanelet && calcAbsYawDiffBetweenLaneletAndObject(object, lanelet) < yaw_threshold;
+    }
+    if (within_lanelet) {
       return true;
     }
   }
@@ -1098,7 +1085,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       attr.value() == lanelet::AttributeValueString::Walkway) {
       const auto & crosswalk = lanelet;
       surrounding_crosswalks.push_back(crosswalk);
-      if (withinLanelet(object, crosswalk)) {
+      if (std::abs(dist) < 1e-5) {
         crossing_crosswalk = crosswalk;
       }
     }
@@ -1184,8 +1171,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     }
 
     const auto reachable_crosswalk = isReachableCrosswalkEdgePoints(
-      object, surrounding_lanelets, surrounding_crosswalks, edge_points,
-      prediction_time_horizon_, min_crosswalk_user_velocity_);
+      object, surrounding_lanelets, surrounding_crosswalks, edge_points, prediction_time_horizon_,
+      min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
       continue;

From 0159c9221fde259ad39b80dda7680e9bd331d711 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:13:00 +0900
Subject: [PATCH 11/16] perf PR 8490

---
 .../src/map_based_prediction_node.cpp         | 40 +++++++++----------
 1 file changed, 20 insertions(+), 20 deletions(-)

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 f740469a50f12..552e51ccc1b5b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -33,6 +33,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <tf2/utils.h>
@@ -360,24 +361,11 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
 }
 
 bool withinRoadLanelet(
-  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const TrackedObject & object,
+  const std::vector<std::pair<double, lanelet::Lanelet>> & surrounding_lanelets_with_dist,
   const bool use_yaw_information = false)
 {
-  using Point = boost::geometry::model::d2::point_xy<double>;
-
-  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  const Point p_object{obj_pos.x, obj_pos.y};
-
-  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
-  // nearest lanelet
-  constexpr double search_radius = 10.0;  // [m]
-  const auto surrounding_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
-
-  for (const auto & lanelet_with_dist : surrounding_lanelets) {
-    const auto & dist = lanelet_with_dist.first;
-    const auto & lanelet = lanelet_with_dist.second;
-
+  for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
     if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
       lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (
@@ -401,6 +389,20 @@ bool withinRoadLanelet(
   return false;
 }
 
+bool withinRoadLanelet(
+  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const bool use_yaw_information = false)
+{
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
+  // nearest lanelet
+  constexpr double search_radius = 10.0;  // [m]
+  const auto surrounding_lanelets_with_dist =
+    lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius);
+
+  return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information);
+}
+
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
   const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points,
@@ -1070,9 +1072,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
-  // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
-  // implementation has been wrong.
-  const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
+  const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d(
     lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
     prediction_time_horizon_ * velocity);
   lanelet::ConstLanelets surrounding_lanelets;
@@ -1117,7 +1117,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
     // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest
     // crosswalk and generate path to the crosswalk edge
-  } else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
+  } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) {
     lanelet::ConstLanelet closest_crosswalk{};
     const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
     const auto found_closest_crosswalk =

From 2a1d6d26b9f287379efeb3ed2e0535c02c240636 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:17:45 +0900
Subject: [PATCH 12/16] perf PR 8751

---
 .../src/path_generator.cpp                    | 30 ++++++++++++-------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp
index a7dda00dc4f21..0850c55b3c248 100644
--- a/perception/map_based_prediction/src/path_generator.cpp
+++ b/perception/map_based_prediction/src/path_generator.cpp
@@ -223,16 +223,18 @@ FrenetPath PathGenerator::generateFrenetPath(
 
   path.reserve(static_cast<size_t>(duration / sampling_time_interval_));
   for (double t = 0.0; t <= duration; t += sampling_time_interval_) {
+    const auto t2 = t * t;
+    const auto t3 = t2 * t;
+    const auto t4 = t3 * t;
+    const auto t5 = t4 * t;
     const double current_acc =
       0.0;  // Currently we assume the object is traveling at a constant speed
-    const double d_next_ = current_point.d + current_point.d_vel * t +
-                           current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) +
-                           lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5);
+    const double d_next_ = current_point.d + current_point.d_vel * t + current_acc * 2.0 * t2 +
+                           lat_coeff(0) * t3 + lat_coeff(1) * t4 + lat_coeff(2) * t5;
     // t > lateral_duration: 0.0, else d_next_
     const double d_next = t > lateral_duration ? 0.0 : d_next_;
-    const double s_next = current_point.s + current_point.s_vel * t +
-                          2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) +
-                          lon_coeff(1) * std::pow(t, 4);
+    const double s_next = current_point.s + current_point.s_vel * t + 2.0 * current_acc * t2 +
+                          lon_coeff(0) * t3 + lon_coeff(1) * t4;
     if (s_next > max_length) {
       break;
     }
@@ -265,10 +267,14 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients(
   // b = np.matrix([[xe - self.a0 - self.a1 * T - self.a2 * T**2],
   //                [vxe - self.a1 - 2 * self.a2 * T],
   //                [axe - 2 * self.a2]])
+  const auto T2 = T * T;
+  const auto T3 = T2 * T;
+  const auto T4 = T3 * T;
+  const auto T5 = T4 * T;
+
   Eigen::Matrix3d A_lat_inv;
-  A_lat_inv << 10 / std::pow(T, 3), -4 / std::pow(T, 2), 1 / (2 * T), -15 / std::pow(T, 4),
-    7 / std::pow(T, 3), -1 / std::pow(T, 2), 6 / std::pow(T, 5), -3 / std::pow(T, 4),
-    1 / (2 * std::pow(T, 3));
+  A_lat_inv << 10 / T3, -4 / T2, 1 / (2 * T), -15 / T4, 7 / T3, -1 / T2, 6 / T5, -3 / T4,
+    1 / (2 * T3);
   Eigen::Vector3d b_lat;
   b_lat[0] = target_point.d - current_point.d - current_point.d_vel * T;
   b_lat[1] = target_point.d_vel - current_point.d_vel;
@@ -286,9 +292,11 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients(
   //                         [-1/(2*T**3), 1/(4*T**2)]])
   // b = np.matrix([[vxe - self.a1 - 2 * self.a2 * T],
   //               [axe - 2 * self.a2]])
+  const auto T2 = T * T;
+  const auto T3 = T2 * T;
+
   Eigen::Matrix2d A_lon_inv;
-  A_lon_inv << 1 / std::pow(T, 2), -1 / (3 * T), -1 / (2 * std::pow(T, 3)),
-    1 / (4 * std::pow(T, 2));
+  A_lon_inv << 1 / T2, -1 / (3 * T), -1 / (2 * T3), 1 / (4 * T2);
   Eigen::Vector2d b_lon;
   b_lon[0] = target_point.s_vel - current_point.s_vel;
   b_lon[1] = 0.0;

From 60cf148812e3ec4eac779eaac74053e52362e6f9 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 21 Oct 2024 16:27:26 +0900
Subject: [PATCH 13/16] chore: fix format

---
 perception/map_based_prediction/src/path_generator.cpp | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp
index 0850c55b3c248..e9348532ec1b9 100644
--- a/perception/map_based_prediction/src/path_generator.cpp
+++ b/perception/map_based_prediction/src/path_generator.cpp
@@ -345,8 +345,8 @@ PosePath PathGenerator::interpolateReferencePath(
     geometry_msgs::msg::Pose interpolated_pose;
     const auto current_point =
       tier4_autoware_utils::createPoint(lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), 0.0);
-    const auto next_point = tier4_autoware_utils::createPoint(
-      lerp_ref_path_x.at(i + 1), lerp_ref_path_y.at(i + 1), 0.0);
+    const auto next_point =
+      tier4_autoware_utils::createPoint(lerp_ref_path_x.at(i + 1), lerp_ref_path_y.at(i + 1), 0.0);
     const double yaw = tier4_autoware_utils::calcAzimuthAngle(current_point, next_point);
     interpolated_pose.position = tier4_autoware_utils::createPoint(
       lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), lerp_ref_path_z.at(i));
@@ -357,7 +357,6 @@ PosePath PathGenerator::interpolateReferencePath(
     lerp_ref_path_x.back(), lerp_ref_path_y.back(), lerp_ref_path_z.back());
   interpolated_path.back().orientation = interpolated_path.at(interpolate_num - 2).orientation;
 
-
   return interpolated_path;
 }
 

From b864937d6e2fe575516e81d040e790030c27efa5 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 22 Oct 2024 13:26:08 +0900
Subject: [PATCH 14/16] perf PR 8657

---
 .../multi_object_tracker_core.hpp             |  8 +++-
 .../src/multi_object_tracker_core.cpp         | 44 ++++++++++++++-----
 2 files changed, 40 insertions(+), 12 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
index 95d33b78ff184..7e9f3a8754695 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
@@ -98,6 +98,11 @@ class MultiObjectTracker : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     detected_object_sub_;
   rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
+  rclcpp::Time last_published_time_;
+  rclcpp::Time last_updated_time_;
+  double publisher_period_;
+  static constexpr double minimum_publish_interval_ratio = 0.85;
+  static constexpr double maximum_publish_interval_ratio = 1.05;
 
   // debugger class
   std::unique_ptr<TrackerDebugger> debugger_;
@@ -116,8 +121,7 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
 
   void checkTrackerLifeCycle(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform);
+    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   void sanitizeTracker(
     std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   std::shared_ptr<Tracker> createNewTracker(
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index d28833241bd5f..2acc4f447cb50 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -174,6 +174,8 @@ void TrackerDebugger::startMeasurementTime(const rclcpp::Time & measurement_head
 
 MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 : rclcpp::Node("multi_object_tracker", node_options),
+  last_published_time_(this->now()),
+  last_updated_time_(this->now()),
   tf_buffer_(this->get_clock()),
   tf_listener_(tf_buffer_)
 {
@@ -196,11 +198,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     this->get_node_base_interface(), this->get_node_timers_interface());
   tf_buffer_.setCreateTimerInterface(cti);
 
-  // Create ROS time based timer
+  // Create ROS time based timer.
+  // If the delay compensation is enabled, the timer is used to publish the output at the correct
+  // time.
   if (enable_delay_compensation) {
-    const auto period_ns = rclcpp::Rate(publish_rate).period();
+    publisher_period_ = 1.0 / publish_rate;    // [s]
+    constexpr double timer_multiplier = 10.0;  // 10 times frequent for publish timing check
+    const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
     publish_timer_ = rclcpp::create_timer(
-      this, get_clock(), period_ns, std::bind(&MultiObjectTracker::onTimer, this));
+      this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
   }
 
   const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
@@ -236,6 +242,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 void MultiObjectTracker::onMeasurement(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
 {
+  last_updated_time_ = this->now();
+
   /* keep the latest input stamp and check transform*/
   debugger_->startMeasurementTime(rclcpp::Time(input_objects_msg->header.stamp));
   const auto self_transform = getTransformAnonymous(
@@ -277,7 +285,7 @@ void MultiObjectTracker::onMeasurement(
   }
 
   /* life cycle check */
-  checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform);
+  checkTrackerLifeCycle(list_tracker_, measurement_time);
   /* sanitize trackers */
   sanitizeTracker(list_tracker_, measurement_time);
 
@@ -326,14 +334,28 @@ std::shared_ptr<Tracker> MultiObjectTracker::createNewTracker(
 void MultiObjectTracker::onTimer()
 {
   rclcpp::Time current_time = this->now();
-  const auto self_transform =
-    getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time);
-  if (!self_transform) {
+
+  // ensure minimum interval: room for the next process(prediction)
+  const double minimum_publish_interval = publisher_period_ * minimum_publish_interval_ratio;
+  const auto elapsed_time = (current_time - last_published_time_).seconds();
+  if (elapsed_time < minimum_publish_interval) {
+    return;
+  }
+
+  // if there was update after publishing, publish new messages
+  bool should_publish = last_published_time_ < last_updated_time_;
+
+  // if there was no update, publish if the elapsed time is longer than the maximum publish latency
+  // in this case, it will perform extrapolate/remove old objects
+  const double maximum_publish_interval = publisher_period_ * maximum_publish_interval_ratio;
+  should_publish = should_publish || elapsed_time > maximum_publish_interval;
+
+  if (!should_publish) {
     return;
   }
 
   /* life cycle check */
-  checkTrackerLifeCycle(list_tracker_, current_time, *self_transform);
+  checkTrackerLifeCycle(list_tracker_, current_time);
   /* sanitize trackers */
   sanitizeTracker(list_tracker_, current_time);
 
@@ -342,8 +364,7 @@ void MultiObjectTracker::onTimer()
 }
 
 void MultiObjectTracker::checkTrackerLifeCycle(
-  std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-  [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform)
+  std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time)
 {
   /* params */
   constexpr float max_elapsed_time = 1.0;
@@ -464,6 +485,9 @@ void MultiObjectTracker::publish(const rclcpp::Time & time)
   // Publish
   tracked_objects_pub_->publish(output_msg);
 
+  // Update last published time
+  last_published_time_ = this->now();
+
   // Debugger Publish if enabled
   debugger_->publishProcessingTime();
   debugger_->publishTentativeObjects(tentative_objects_msg);

From 5de95b01c51cabadc42cf11f83a534ff7cf4f11e Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 22 Oct 2024 15:16:06 +0900
Subject: [PATCH 15/16] feat: improve lanelet search logic in
 getPredictedReferencePath()

---
 .../src/map_based_prediction_node.cpp                    | 9 +++++++++
 1 file changed, 9 insertions(+)

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 552e51ccc1b5b..499115f07f4fc 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1550,6 +1550,15 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
         if (!unconnected_lanelets.empty()) {
           return unconnected_lanelets.front();
         }
+        // search side of the next lanelet
+        const lanelet::ConstLanelets next_lanelet = routing_graph_ptr_->following(lanelet);
+        if (!next_lanelet.empty()) {
+          const auto next = get_left ? routing_graph_ptr_->left(next_lanelet.front())
+                                     : routing_graph_ptr_->right(next_lanelet.front());
+          if (!!next) {
+            return *next;
+          }
+        }
       }
 
       // if no candidate lanelet found, return empty

From 4fb0d67bbcf03af94c08019b49a06bb3c60506ba Mon Sep 17 00:00:00 2001
From: Taekjin LEE <technolojin@gmail.com>
Date: Tue, 29 Oct 2024 15:54:22 +0900
Subject: [PATCH 16/16] sp develop remove non approved change (#1611)

Revert "feat: improve lanelet search logic in getPredictedReferencePath()"

This reverts commit 5de95b01c51cabadc42cf11f83a534ff7cf4f11e.
---
 .../src/map_based_prediction_node.cpp                    | 9 ---------
 1 file changed, 9 deletions(-)

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 499115f07f4fc..552e51ccc1b5b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1550,15 +1550,6 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
         if (!unconnected_lanelets.empty()) {
           return unconnected_lanelets.front();
         }
-        // search side of the next lanelet
-        const lanelet::ConstLanelets next_lanelet = routing_graph_ptr_->following(lanelet);
-        if (!next_lanelet.empty()) {
-          const auto next = get_left ? routing_graph_ptr_->left(next_lanelet.front())
-                                     : routing_graph_ptr_->right(next_lanelet.front());
-          if (!!next) {
-            return *next;
-          }
-        }
       }
 
       // if no candidate lanelet found, return empty