diff --git a/common/autoware_universe_utils/examples/example_lru_cache.cpp b/common/autoware_universe_utils/examples/example_lru_cache.cpp
new file mode 100644
index 0000000000000..95decf8eb336d
--- /dev/null
+++ b/common/autoware_universe_utils/examples/example_lru_cache.cpp
@@ -0,0 +1,73 @@
+// 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 "autoware/universe_utils/system/lru_cache.hpp"
+
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+
+using autoware::universe_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>
+int64_t measure_time(Func func, Args &&... args)
+{
+  auto start = std::chrono::high_resolution_clock::now();
+  func(std::forward<Args>(args)...);
+  auto end = std::chrono::high_resolution_clock::now();
+  return std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
+}
+
+int main()
+{
+  const int max_n = 40;                 // Increased for more noticeable time difference
+  LRUCache<int, int64_t> cache(max_n);  // Cache size set to MAX_N
+
+  std::cout << "Comparing Fibonacci calculation time with and without LRU cache:\n\n";
+  std::cout << "n\tWith Cache (μs)\tWithout Cache (μs)\n";
+  std::cout << std::string(43, '-') << "\n";
+
+  for (int i = 30; i <= max_n; ++i) {  // Starting from 30 for more significant differences
+    int64_t time_with_cache = measure_time([i, &cache]() { fibonacci_with_cache(i, &cache); });
+    int64_t time_without_cache = measure_time([i]() { fibonacci_no_cache(i); });
+
+    std::cout << i << "\t" << time_with_cache << "\t\t" << time_without_cache << "\n";
+
+    // Clear the cache after each iteration to ensure fair comparison
+    cache.clear();
+  }
+
+  return 0;
+}
diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/system/lru_cache.hpp
new file mode 100644
index 0000000000000..ecd8fc7e6c01a
--- /dev/null
+++ b/common/autoware_universe_utils/include/autoware/universe_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 AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_
+
+#include <cstddef>
+#include <list>
+#include <optional>
+#include <unordered_map>
+#include <utility>
+
+namespace autoware::universe_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 autoware::universe_utils
+
+#endif  // AUTOWARE__UNIVERSE_UTILS__SYSTEM__LRU_CACHE_HPP_
diff --git a/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp b/common/autoware_universe_utils/test/src/system/test_lru_cache.cpp
new file mode 100644
index 0000000000000..f30f732431fa6
--- /dev/null
+++ b/common/autoware_universe_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 "autoware/universe_utils/system/lru_cache.hpp"
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <utility>
+
+using autoware::universe_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();
+  }
+}
diff --git a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml
index a5b7b0ad6be01..b45cc25f82c1c 100644
--- a/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml
+++ b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml
@@ -50,3 +50,8 @@
       consider_only_routable_neighbours: false
 
     reference_path_resolution: 0.5 #[m]
+
+    # debug parameters
+    publish_processing_time: false
+    publish_processing_time_detail: false
+    publish_debug_markers: false
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 1c677c9d0f6e4..3078fe89444b8 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>
@@ -39,7 +40,9 @@
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #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>
@@ -53,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
@@ -178,6 +202,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Crosswalk Entry Points
   lanelet::ConstLanelets crosswalks_;
 
+  // Fences
+  lanelet::LaneletMapUPtr fence_layer_{nullptr};
+
   // Parameters
   bool enable_delay_compensation_;
   PredictionTimeHorizon prediction_time_horizon_;
@@ -225,7 +252,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
   rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
     detailed_processing_time_publisher_;
-  mutable autoware::universe_utils::TimeKeeper time_keeper_;
+  std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_;
 
   // Member Functions
   void mapCallback(const LaneletMapBin::ConstSharedPtr msg);
@@ -287,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);
@@ -313,6 +343,10 @@ class MapBasedPredictionNode : public rclcpp::Node
     const TrackedObject & object, const LaneletData & current_lanelet_data,
     const double object_detected_time);
 
+  void publish(
+    const PredictedObjects & output,
+    const visualization_msgs::msg::MarkerArray & debug_markers) const;
+
   // NOTE: This function is copied from the motion_velocity_smoother package.
   // TODO(someone): Consolidate functions and move them to a common
   inline std::vector<double> calcTrajectoryCurvatureFrom3Points(
diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp
index bd098894f8a88..8861598ae7fb2 100644
--- a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp
+++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp
@@ -16,6 +16,7 @@
 #define MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_
 
 #include <Eigen/Eigen>
+#include <autoware/universe_utils/system/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
@@ -82,6 +83,8 @@ class PathGenerator
 public:
   PathGenerator(const double sampling_time_interval, const double min_crosswalk_user_velocity);
 
+  void setTimeKeeper(std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_ptr);
+
   PredictedPath generatePathForNonVehicleObject(
     const TrackedObject & object, const double duration) const;
 
@@ -119,6 +122,8 @@ class PathGenerator
   bool use_vehicle_acceleration_;
   double acceleration_exponential_half_life_;
 
+  std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_;
+
   // Member functions
   PredictedPath generateStraightPath(const TrackedObject & object, const double duration) const;
 
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 29cfe52855d24..8c595c75eb481 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
@@ -52,6 +52,7 @@
 
 namespace autoware::map_based_prediction
 {
+using autoware::universe_utils::ScopedTimeTrack;
 
 namespace
 {
@@ -358,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)
@@ -391,9 +370,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) {
@@ -401,7 +383,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;
     }
   }
@@ -411,8 +399,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>;
 
@@ -439,10 +427,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;
         }
@@ -662,7 +650,6 @@ ObjectClassification::_label_type changeLabelForPrediction(
     }
 
     case ObjectClassification::PEDESTRIAN: {
-      const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true);
       const float max_velocity_for_human_mps =
         autoware::universe_utils::kmph2mps(25.0);  // Max human being motion speed is 25km/h
       const double abs_speed = std::hypot(
@@ -670,7 +657,6 @@ ObjectClassification::_label_type changeLabelForPrediction(
         object.kinematics.twist_with_covariance.twist.linear.y);
       const bool high_speed_object = abs_speed > max_velocity_for_human_mps;
       // fast, human-like object: like segway
-      if (within_road_lanelet && high_speed_object) return label;  // currently do nothing
       // return ObjectClassification::MOTORCYCLE;
       if (high_speed_object) return label;  // currently do nothing
       // fast human outside road lanelet will move like unknown object
@@ -833,12 +819,18 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   timeout_set_for_no_intention_to_walk_ = declare_parameter<std::vector<double>>(
     "crosswalk_with_signal.timeout_set_for_no_intention_to_walk");
 
+  // debug parameter
+  bool use_time_publisher = declare_parameter<bool>("publish_processing_time");
+  bool use_time_keeper = declare_parameter<bool>("publish_processing_time_detail");
+  bool use_debug_marker = declare_parameter<bool>("publish_debug_markers");
+
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
 
   path_generator_->setUseVehicleAcceleration(use_vehicle_acceleration_);
   path_generator_->setAccelerationHalfLife(acceleration_exponential_half_life_);
 
+  // subscribers
   sub_objects_ = this->create_subscription<TrackedObjects>(
     "~/input/objects", 1,
     std::bind(&MapBasedPredictionNode::objectsCallback, this, std::placeholders::_1));
@@ -846,26 +838,37 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     "/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1));
 
+  // publishers
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
-  pub_debug_markers_ =
-    this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
-  processing_time_publisher_ =
-    std::make_unique<autoware::universe_utils::DebugPublisher>(this, "map_based_prediction");
-
-  published_time_publisher_ =
-    std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
-  detailed_processing_time_publisher_ =
-    this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
-      "~/debug/processing_time_detail_ms", 1);
-  time_keeper_ = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_);
 
+  // debug publishers
+  if (use_time_publisher) {
+    processing_time_publisher_ =
+      std::make_unique<autoware::universe_utils::DebugPublisher>(this, "map_based_prediction");
+    published_time_publisher_ =
+      std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
+    stop_watch_ptr_ =
+      std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
+  if (use_time_keeper) {
+    detailed_processing_time_publisher_ =
+      this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+        "~/debug/processing_time_detail_ms", 1);
+    auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_);
+    time_keeper_ = std::make_shared<autoware::universe_utils::TimeKeeper>(time_keeper);
+    path_generator_->setTimeKeeper(time_keeper_);
+  }
+
+  if (use_debug_marker) {
+    pub_debug_markers_ =
+      this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
+  }
+  // dynamic reconfigure
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
-
-  stop_watch_ptr_ =
-    std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
-  stop_watch_ptr_->tic("cyclic_time");
-  stop_watch_ptr_->tic("processing_time");
 }
 
 rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
@@ -918,12 +921,11 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject(
 
 void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet");
   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_);
@@ -931,12 +933,23 @@ void MapBasedPredictionNode::mapCallback(const LaneletMapBin::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::trafficSignalsCallback(
   const TrafficLightGroupArray::ConstSharedPtr msg)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   traffic_signal_id_map_.clear();
   for (const auto & signal : msg->traffic_light_groups) {
@@ -946,9 +959,10 @@ void MapBasedPredictionNode::trafficSignalsCallback(
 
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
-  stop_watch_ptr_->toc("processing_time", true);
+  if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true);
 
   // take traffic_signal
   {
@@ -963,23 +977,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, object_buffer_time_length_, road_users_history);
@@ -1018,12 +1015,24 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   }
   std::unordered_set<std::string> predicted_crosswalk_users_ids;
 
+  // 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 = autoware::universe_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;
@@ -1112,14 +1121,16 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         }
 
         // Get Debug Marker for On Lane Vehicles
-        const auto max_prob_path = std::max_element(
-          ref_paths.begin(), ref_paths.end(),
-          [](const PredictedRefPath & a, const PredictedRefPath & b) {
-            return a.probability < b.probability;
-          });
-        const auto debug_marker =
-          getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size());
-        debug_markers.markers.push_back(debug_marker);
+        if (pub_debug_markers_) {
+          const auto max_prob_path = std::max_element(
+            ref_paths.begin(), ref_paths.end(),
+            [](const PredictedRefPath & a, const PredictedRefPath & b) {
+              return a.probability < b.probability;
+            });
+          const auto debug_marker =
+            getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size());
+          debug_markers.markers.push_back(debug_marker);
+        }
 
         // Fix object angle if its orientation unreliable (e.g. far object by radar sensor)
         // This prevent bending predicted path
@@ -1225,21 +1236,36 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   }
 
   // Publish Results
+  publish(output, debug_markers);
+
+  // Publish Processing Time
+  if (stop_watch_ptr_) {
+    const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+    const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
+}
+
+void MapBasedPredictionNode::publish(
+  const PredictedObjects & output, const visualization_msgs::msg::MarkerArray & debug_markers) const
+{
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   pub_objects_->publish(output);
-  published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
-  pub_debug_markers_->publish(debug_markers);
-  const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
-  const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
-  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
-    "debug/cyclic_time_ms", cyclic_time_ms);
-  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
-    "debug/processing_time_ms", processing_time_ms);
+  if (published_time_publisher_)
+    published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
+  if (pub_debug_markers_) pub_debug_markers_->publish(debug_markers);
 }
 
 void MapBasedPredictionNode::updateCrosswalkUserHistory(
   const std_msgs::msg::Header & header, const TrackedObject & object, const std::string & object_id)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   CrosswalkUserData crosswalk_user_data;
   crosswalk_user_data.header = header;
@@ -1256,7 +1282,8 @@ void MapBasedPredictionNode::updateCrosswalkUserHistory(
 std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
   const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   const auto known_match_opt = [&]() -> std::optional<std::string> {
     if (!known_matches_.count(object_id)) {
@@ -1315,11 +1342,16 @@ std::string MapBasedPredictionNode::tryMatchNewObjectToDisappeared(
 
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-  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)) {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
+  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 =
+    fence_layer_->lineStringLayer.search(lanelet::geometry::boundingBox2d(predicted_path_ls));
+  for (const auto & candidate : candidates) {
+    if (doesPathCrossFence(predicted_path, candidate)) {
       return true;
     }
   }
@@ -1329,7 +1361,8 @@ bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predict
 bool MapBasedPredictionNode::doesPathCrossFence(
   const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // check whether the predicted path cross with fence
   for (size_t i = 0; i < predicted_path.path.size() - 1; ++i) {
@@ -1348,6 +1381,9 @@ bool MapBasedPredictionNode::isIntersecting(
   const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2,
   const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   const auto p1 = autoware::universe_utils::createPoint(point1.x, point1.y, 0.0);
   const auto p2 = autoware::universe_utils::createPoint(point2.x, point2.y, 0.0);
   const auto p3 = autoware::universe_utils::createPoint(point3.x(), point3.y(), 0.0);
@@ -1359,7 +1395,8 @@ bool MapBasedPredictionNode::isIntersecting(
 PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   const TrackedObject & object)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   auto predicted_object = convertToPredictedObject(object);
   {
@@ -1381,7 +1418,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
     lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
     prediction_time_horizon_.pedestrian * 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);
@@ -1389,10 +1426,9 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
       attr.value() == lanelet::AttributeValueString::Crosswalk ||
       attr.value() == lanelet::AttributeValueString::Walkway) {
       const auto & crosswalk = lanelet;
-      if (withinLanelet(object, crosswalk)) {
+      surrounding_crosswalks.push_back(crosswalk);
+      if (std::abs(dist) < 1e-5) {
         crossing_crosswalk = crosswalk;
-      } else {
-        external_surrounding_crosswalks.push_back(crosswalk);
       }
     }
   }
@@ -1456,7 +1492,10 @@ 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 crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
     if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
       if (!calcIntentionToCrossWithTrafficSignal(
@@ -1481,7 +1520,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_.pedestrian, min_crosswalk_user_velocity_);
 
     if (!reachable_crosswalk) {
@@ -1513,7 +1552,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
 void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   if (
     object.kinematics.orientation_availability ==
@@ -1565,8 +1605,6 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object)
 void MapBasedPredictionNode::removeStaleTrafficLightInfo(
   const TrackedObjects::ConstSharedPtr in_objects)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   for (auto it = stopped_times_against_green_.begin(); it != stopped_times_against_green_.end();) {
     const bool isDisappeared = std::none_of(
       in_objects->objects.begin(), in_objects->objects.end(),
@@ -1583,7 +1621,8 @@ void MapBasedPredictionNode::removeStaleTrafficLightInfo(
 
 LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & object)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // obstacle point
   lanelet::BasicPoint2d search_point(
@@ -1675,7 +1714,8 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob
 bool MapBasedPredictionNode::checkCloseLaneletCondition(
   const std::pair<double, lanelet::Lanelet> & lanelet, const TrackedObject & object)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // Step1. If we only have one point in the centerline, we will ignore the lanelet
   if (lanelet.second.centerline().size() <= 1) {
@@ -1723,7 +1763,8 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
 float MapBasedPredictionNode::calculateLocalLikelihood(
   const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   const auto & obj_point = object.kinematics.pose_with_covariance.pose.position;
 
@@ -1760,7 +1801,8 @@ void MapBasedPredictionNode::updateRoadUsersHistory(
   const std_msgs::msg::Header & header, const TrackedObject & object,
   const LaneletsData & current_lanelets_data)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   std::string object_id = autoware::universe_utils::toHexString(object.object_id);
   const auto current_lanelets = getLanelets(current_lanelets_data);
@@ -1803,7 +1845,8 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
   const TrackedObject & object, const LaneletsData & current_lanelets_data,
   const double object_detected_time, const double time_horizon)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   const double obj_vel = std::hypot(
     object.kinematics.twist_with_covariance.twist.linear.x,
@@ -1966,7 +2009,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
   const double object_detected_time)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // calculate maneuver
   const auto current_maneuver = [&]() {
@@ -2018,7 +2062,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByTimeToLaneChange(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
   const double /*object_detected_time*/)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // Step1. Check if we have the object in the buffer
   const std::string object_id = autoware::universe_utils::toHexString(object.object_id);
@@ -2091,7 +2136,8 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
   const TrackedObject & object, const LaneletData & current_lanelet_data,
   const double /*object_detected_time*/)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   // Step1. Check if we have the object in the buffer
   const std::string object_id = autoware::universe_utils::toHexString(object.object_id);
@@ -2236,8 +2282,6 @@ geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay(
 double MapBasedPredictionNode::calcRightLateralOffset(
   const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   std::vector<geometry_msgs::msg::Point> boundary_path(boundary_line.size());
   for (size_t i = 0; i < boundary_path.size(); ++i) {
     const double x = boundary_line[i].x();
@@ -2257,7 +2301,8 @@ double MapBasedPredictionNode::calcLeftLateralOffset(
 void MapBasedPredictionNode::updateFuturePossibleLanelets(
   const TrackedObject & object, const lanelet::routing::LaneletPaths & paths)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   std::string object_id = autoware::universe_utils::toHexString(object.object_id);
   if (road_users_history.count(object_id) == 0) {
@@ -2283,7 +2328,8 @@ void MapBasedPredictionNode::addReferencePaths(
   const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
   const double speed_limit)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   if (!candidate_paths.empty()) {
     updateFuturePossibleLanelets(object, candidate_paths);
@@ -2304,7 +2350,8 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability(
   const lanelet::routing::LaneletPaths & right_paths,
   const lanelet::routing::LaneletPaths & center_paths)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   float left_lane_change_probability = 0.0;
   float right_lane_change_probability = 0.0;
@@ -2366,9 +2413,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_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__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) {
@@ -2392,7 +2444,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 = autoware::universe_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;
         }
@@ -2423,18 +2481,31 @@ 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 = autoware::universe_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 =
-      autoware::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 = 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;
 }
 
@@ -2442,8 +2513,6 @@ bool MapBasedPredictionNode::isDuplicated(
   const std::pair<double, lanelet::ConstLanelet> & target_lanelet,
   const LaneletsData & lanelets_data)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   const double CLOSE_LANELET_THRESHOLD = 0.1;
   for (const auto & lanelet_data : lanelets_data) {
     const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back();
@@ -2461,8 +2530,6 @@ bool MapBasedPredictionNode::isDuplicated(
 bool MapBasedPredictionNode::isDuplicated(
   const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   const double CLOSE_PATH_THRESHOLD = 0.1;
   for (const auto & prev_predicted_path : predicted_paths) {
     const auto prev_path_end = prev_predicted_path.path.back().position;
@@ -2479,8 +2546,6 @@ bool MapBasedPredictionNode::isDuplicated(
 std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
   const lanelet::ConstLanelet & way_lanelet)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
-
   const auto traffic_light_reg_elems =
     way_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>();
   if (traffic_light_reg_elems.empty()) {
@@ -2497,7 +2562,8 @@ std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
 std::optional<TrafficLightElement> MapBasedPredictionNode::getTrafficSignalElement(
   const lanelet::Id & id)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   if (traffic_signal_id_map_.count(id) != 0) {
     const auto & signal_elements = traffic_signal_id_map_.at(id).elements;
@@ -2515,7 +2581,8 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal(
   const TrackedObject & object, const lanelet::ConstLanelet & crosswalk,
   const lanelet::Id & signal_id)
 {
-  autoware::universe_utils::ScopedTimeTrack st(__func__, time_keeper_);
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
   const auto signal_color = [&] {
     const auto elem_opt = getTrafficSignalElement(signal_id);
diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp
index aeb1b0fedd33f..6c61876176b68 100644
--- a/perception/autoware_map_based_prediction/src/path_generator.cpp
+++ b/perception/autoware_map_based_prediction/src/path_generator.cpp
@@ -16,12 +16,14 @@
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
-#include <interpolation/spline_interpolation.hpp>
+#include <interpolation/linear_interpolation.hpp>
 
 #include <algorithm>
 
 namespace autoware::map_based_prediction
 {
+using autoware::universe_utils::ScopedTimeTrack;
+
 PathGenerator::PathGenerator(
   const double sampling_time_interval, const double min_crosswalk_user_velocity)
 : sampling_time_interval_(sampling_time_interval),
@@ -29,15 +31,27 @@ PathGenerator::PathGenerator(
 {
 }
 
+void PathGenerator::setTimeKeeper(
+  std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_ptr)
+{
+  time_keeper_ = std::move(time_keeper_ptr);
+}
+
 PredictedPath PathGenerator::generatePathForNonVehicleObject(
   const TrackedObject & object, const double duration) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   return generateStraightPath(object, duration);
 }
 
 PredictedPath PathGenerator::generatePathToTargetPoint(
   const TrackedObject & object, const Eigen::Vector2d & point) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   PredictedPath predicted_path{};
   const double ep = 0.001;
 
@@ -77,6 +91,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
   const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
   const double duration) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   PredictedPath predicted_path{};
   const double ep = 0.001;
 
@@ -135,6 +152,9 @@ PredictedPath PathGenerator::generatePathForCrosswalkUser(
 PredictedPath PathGenerator::generatePathForLowSpeedVehicle(
   const TrackedObject & object, const double duration) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   PredictedPath path;
   path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_);
   const double ep = 0.001;
@@ -148,6 +168,9 @@ PredictedPath PathGenerator::generatePathForLowSpeedVehicle(
 PredictedPath PathGenerator::generatePathForOffLaneVehicle(
   const TrackedObject & object, const double duration) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   return generateStraightPath(object, duration);
 }
 
@@ -155,6 +178,9 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle(
   const TrackedObject & object, const PosePath & ref_paths, const double duration,
   const double lateral_duration, const double speed_limit) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   if (ref_paths.size() < 2) {
     return generateStraightPath(object, duration);
   }
@@ -186,9 +212,12 @@ PredictedPath PathGenerator::generatePolynomialPath(
   const TrackedObject & object, const PosePath & ref_path, const double duration,
   const double lateral_duration, const double speed_limit) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   // Get current Frenet Point
   const double ref_path_len = autoware::motion_utils::calcArcLength(ref_path);
-  const auto current_point = getFrenetPoint(object, ref_path, speed_limit, duration);
+  const auto current_point = getFrenetPoint(object, ref_path, duration, speed_limit);
 
   // Step1. Set Target Frenet Point
   // Note that we do not set position s,
@@ -219,6 +248,9 @@ FrenetPath PathGenerator::generateFrenetPath(
   const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
   const double duration, const double lateral_duration) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   FrenetPath path;
 
   // Compute Lateral and Longitudinal Coefficients to generate the trajectory
@@ -303,6 +335,9 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients(
 PosePath PathGenerator::interpolateReferencePath(
   const PosePath & base_path, const FrenetPath & frenet_predicted_path) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   PosePath interpolated_path;
   const size_t interpolate_num = frenet_predicted_path.size();
   if (interpolate_num < 2) {
@@ -332,29 +367,26 @@ 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 =
-      autoware::universe_utils::createPoint(spline_ref_path_x.at(i), spline_ref_path_y.at(i), 0.0);
+      autoware::universe_utils::createPoint(lerp_ref_path_x.at(i), lerp_ref_path_y.at(i), 0.0);
     const auto next_point = autoware::universe_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 = autoware::universe_utils::calcAzimuthAngle(current_point, next_point);
     interpolated_pose.position = autoware::universe_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 = autoware::universe_utils::createQuaternionFromYaw(yaw);
     interpolated_path.at(i) = interpolated_pose;
   }
   interpolated_path.back().position = autoware::universe_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;
@@ -364,6 +396,9 @@ PredictedPath PathGenerator::convertToPredictedPath(
   const TrackedObject & object, const FrenetPath & frenet_predicted_path,
   const PosePath & ref_path) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   PredictedPath predicted_path;
   predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_);
   predicted_path.path.resize(ref_path.size());
@@ -395,6 +430,9 @@ FrenetPoint PathGenerator::getFrenetPoint(
   const TrackedObject & object, const PosePath & ref_path, const double duration,
   const double speed_limit) const
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   FrenetPoint frenet_point;
   const auto obj_point = object.kinematics.pose_with_covariance.pose.position;