diff --git a/common/interpolation/CMakeLists.txt b/common/interpolation/CMakeLists.txt
index 6c35d1e240f1b..7afed6fe57c41 100644
--- a/common/interpolation/CMakeLists.txt
+++ b/common/interpolation/CMakeLists.txt
@@ -8,7 +8,6 @@ ament_auto_add_library(interpolation SHARED
   src/linear_interpolation.cpp
   src/spline_interpolation.cpp
   src/spline_interpolation_points_2d.cpp
-  src/zero_order_hold.cpp
 )
 
 if(BUILD_TESTING)
diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp
index 0a8f41732916b..7de6743ed9d16 100644
--- a/common/interpolation/include/interpolation/zero_order_hold.hpp
+++ b/common/interpolation/include/interpolation/zero_order_hold.hpp
@@ -21,9 +21,53 @@
 
 namespace interpolation
 {
-std::vector<double> zero_order_hold(
-  const std::vector<double> & base_keys, const std::vector<double> & base_values,
-  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3);
+template <class T>
+std::vector<T> zero_order_hold(
+  const std::vector<double> & base_keys, const std::vector<T> & base_values,
+  const std::vector<double> & query_keys, const double overlap_threshold = 1e-3)
+{
+  // throw exception for invalid arguments
+  interpolation_utils::validateKeys(base_keys, query_keys);
+
+  // when vectors are empty
+  if (base_keys.empty() || base_values.empty()) {
+    throw std::invalid_argument("Points is empty.");
+  }
+
+  // when size of vectors are less than 2
+  if (base_keys.size() < 2 || base_values.size() < 2) {
+    throw std::invalid_argument(
+      "The size of points is less than 2. base_keys.size() = " + std::to_string(base_keys.size()) +
+      ", base_values.size() = " + std::to_string(base_values.size()));
+  }
+
+  // when sizes of indices and values are not same
+  if (base_keys.size() != base_values.size()) {
+    throw std::invalid_argument("The size of base_keys and base_values are not the same.");
+  }
+
+  std::vector<T> query_values;
+  size_t closest_segment_idx = 0;
+  for (size_t i = 0; i < query_keys.size(); ++i) {
+    // Check if query_key is closes to the terminal point of the base keys
+    if (base_keys.back() - overlap_threshold < query_keys.at(i)) {
+      closest_segment_idx = base_keys.size() - 1;
+    } else {
+      for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) {
+        if (
+          base_keys.at(j) - overlap_threshold < query_keys.at(i) &&
+          query_keys.at(i) < base_keys.at(j + 1)) {
+          // find closest segment in base keys
+          closest_segment_idx = j;
+        }
+      }
+    }
+
+    query_values.push_back(base_values.at(closest_segment_idx));
+  }
+
+  return query_values;
+}
 }  // namespace interpolation
 
 #endif  // INTERPOLATION__ZERO_ORDER_HOLD_HPP_
diff --git a/common/interpolation/src/zero_order_hold.cpp b/common/interpolation/src/zero_order_hold.cpp
deleted file mode 100644
index ec590f1c17aed..0000000000000
--- a/common/interpolation/src/zero_order_hold.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-// Copyright 2022 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 "interpolation/zero_order_hold.hpp"
-
-#include <vector>
-
-namespace interpolation
-{
-std::vector<double> zero_order_hold(
-  const std::vector<double> & base_keys, const std::vector<double> & base_values,
-  const std::vector<double> & query_keys, const double overlap_threshold)
-{
-  // throw exception for invalid arguments
-  interpolation_utils::validateKeys(base_keys, query_keys);
-  interpolation_utils::validateKeysAndValues(base_keys, base_values);
-
-  std::vector<double> query_values;
-  size_t closest_segment_idx = 0;
-  for (size_t i = 0; i < query_keys.size(); ++i) {
-    // Check if query_key is closes to the terminal point of the base keys
-    if (base_keys.back() - overlap_threshold < query_keys.at(i)) {
-      closest_segment_idx = base_keys.size() - 1;
-    } else {
-      for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) {
-        if (
-          base_keys.at(j) - overlap_threshold < query_keys.at(i) &&
-          query_keys.at(i) < base_keys.at(j + 1)) {
-          // find closest segment in base keys
-          closest_segment_idx = j;
-        }
-      }
-    }
-
-    query_values.push_back(base_values.at(closest_segment_idx));
-  }
-
-  return query_values;
-}
-
-}  // namespace interpolation
diff --git a/common/interpolation/test/src/test_zero_order_hold.cpp b/common/interpolation/test/src/test_zero_order_hold.cpp
index f11ea01ccc347..541af1cf76064 100644
--- a/common/interpolation/test/src/test_zero_order_hold.cpp
+++ b/common/interpolation/test/src/test_zero_order_hold.cpp
@@ -95,3 +95,54 @@ TEST(zero_order_hold_interpolation, vector_interpolation)
     }
   }
 }
+
+TEST(zero_order_hold_interpolation, vector_interpolation_no_double_interpolation)
+{
+  {  // straight: query_keys is same as base_keys
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, true};
+    const std::vector<double> query_keys = base_keys;
+    const auto ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // straight: query_keys is random
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, false, true, false};
+    const std::vector<double> query_keys{0.0, 0.7, 1.9, 4.0};
+    const std::vector<bool> ans = {true, true, false, false};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_EQ(query_values.at(i), ans.at(i));
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<bool> base_values{true, true, false, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.001};
+    const std::vector<double> ans = {true, true, false, true, true};
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+
+  {  // Boundary Condition
+    const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0, 4.0};
+    const std::vector<double> base_values{true, false, true, true, false};
+    const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0, 4.0 - 0.0001};
+    const std::vector<double> ans = base_values;
+
+    const auto query_values = interpolation::zero_order_hold(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
+}
diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp
index bee91d3a3b08c..0accce28499c0 100644
--- a/common/motion_utils/include/motion_utils/resample/resample.hpp
+++ b/common/motion_utils/include/motion_utils/resample/resample.hpp
@@ -27,6 +27,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_planning_msgs/msg/path.hpp"
+#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 
 #include <algorithm>
@@ -55,6 +56,54 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(
   const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
   const bool use_lerp_for_z = true);
 
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ * are resampled by spline interpolation, position z are resampled by linear interpolation,
+ * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is resampled
+ * by linear interpolation. Orientation of the resampled path are calculated by a forward difference
+ * method based on the interpolated position x and y. Moreover, lane_ids and is_final are also
+ * interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_arclength arclength that contains length of each resampling points from initial
+ * point
+ * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
+ * y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ * Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ * longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @return resampled path
+ */
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);
+
+/**
+ * @brief A resampling function for a path with lane id. Note that in a default setting, position xy
+ * are resampled by spline interpolation, position z are resampled by linear interpolation,
+ * longitudinal and lateral velocity are resampled by zero_order_hold, and heading rate is resampled
+ * by linear interpolation. Orientation of the resampled path are calculated by a forward difference
+ * method based on the interpolated position x and y. Moreover, lane_ids and is_final are also
+ * interpolated by zero order hold
+ * @param input_path input path to resample
+ * @param resampled_interval resampling interval
+ * point
+ * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and
+ * y. Otherwise, it uses spline interpolation
+ * @param use_lerp_for_z If true, it uses linear interpolation to resample position z.
+ * Otherwise, it uses spline interpolation
+ * @param use_zero_order_hold_for_v If true, it uses zero_order_hold to resample
+ * longitudinal and lateral velocity. Otherwise, it uses linear interpolation
+ * @param resample_input_path_stop_point If true, resample closest stop point in input path
+ * @return resampled path
+ */
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const double resample_interval, const bool use_lerp_for_xy = false,
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true,
+  const bool resample_input_path_stop_point = true);
+
 /**
  * @brief A resampling function for a path. Note that in a default setting, position xy are
  * resampled by spline interpolation, position z are resampled by linear interpolation, longitudinal
@@ -116,12 +165,15 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
  * Otherwise, it uses spline interpolation
  * @param use_zero_order_hold_for_twist If true, it uses zero_order_hold to resample
  * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation
+ * @param resample_input_trajectory_stop_point If true, resample closest stop point in input
+ * trajectory
  * @return resampled trajectory
  */
 autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
   const double resample_interval, const bool use_lerp_for_xy = false,
-  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true);
+  const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true,
+  const bool resample_input_trajectory_stop_point = true);
 }  // namespace motion_utils
 
 #endif  // MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp
index a6695d36d8f38..20466806570be 100644
--- a/common/motion_utils/src/resample/resample.cpp
+++ b/common/motion_utils/src/resample/resample.cpp
@@ -115,6 +115,171 @@ std::vector<geometry_msgs::msg::Pose> resamplePath(
   return resampled_points;
 }
 
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
+  const bool use_lerp_for_z, const bool use_zero_order_hold_for_v)
+{
+  // Check vector size and if out_arclength have the end point of the path
+  if (input_path.points.size() < 2 || resampled_arclength.size() < 2) {
+    std::cerr << "[motion_utils]: input path size or input path length is wrong" << std::endl;
+    return input_path;
+  }
+
+  // For LaneIds, is_final
+  //
+  // ------|----|----|----|----|----|----|-------> resampled
+  //      [0]  [1]  [2]  [3]  [4]  [5]  [6]
+  //
+  // ------|----------------|----------|---------> base
+  //      [0]             [1]        [2]
+  //
+  // resampled[0~3] = base[0]
+  // resampled[4~5] = base[1]
+  // resampled[6] = base[2]
+
+  // Input Path Information
+  std::vector<double> input_arclength(input_path.points.size());
+  std::vector<geometry_msgs::msg::Pose> input_pose(input_path.points.size());
+  std::vector<double> v_lon(input_path.points.size());
+  std::vector<double> v_lat(input_path.points.size());
+  std::vector<double> heading_rate(input_path.points.size());
+  std::vector<bool> is_final(input_path.points.size());
+  std::vector<std::vector<int64_t>> lane_ids(input_path.points.size());
+
+  input_arclength.front() = 0.0;
+  input_pose.front() = input_path.points.front().point.pose;
+  v_lon.front() = input_path.points.front().point.longitudinal_velocity_mps;
+  v_lat.front() = input_path.points.front().point.lateral_velocity_mps;
+  heading_rate.front() = input_path.points.front().point.heading_rate_rps;
+  for (size_t i = 1; i < input_path.points.size(); ++i) {
+    const auto & prev_pt = input_path.points.at(i - 1).point;
+    const auto & curr_pt = input_path.points.at(i).point;
+    const double ds =
+      tier4_autoware_utils::calcDistance2d(prev_pt.pose.position, curr_pt.pose.position);
+    input_arclength.at(i) = ds + input_arclength.at(i - 1);
+    input_pose.at(i) = curr_pt.pose;
+    v_lon.at(i) = curr_pt.longitudinal_velocity_mps;
+    v_lat.at(i) = curr_pt.lateral_velocity_mps;
+    heading_rate.at(i) = curr_pt.heading_rate_rps;
+    is_final.at(i) = curr_pt.is_final;
+    lane_ids.at(i) = input_path.points.at(i).lane_ids;
+  }
+
+  if (input_arclength.back() < resampled_arclength.back()) {
+    std::cerr << "[motion_utils]: resampled path length is longer than input path length"
+              << std::endl;
+    return input_path;
+  }
+
+  // Interpolate
+  const auto lerp = [&](const auto & input) {
+    return interpolation::lerp(input_arclength, input, resampled_arclength);
+  };
+  const auto zoh = [&](const auto & input) {
+    return interpolation::zero_order_hold(input_arclength, input, resampled_arclength);
+  };
+
+  const auto interpolated_pose =
+    resamplePath(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z);
+  const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon);
+  const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat);
+  const auto interpolated_heading_rate = lerp(heading_rate);
+  const auto interpolated_is_final = zoh(is_final);
+  const auto interpolated_lane_ids = zoh(lane_ids);
+
+  if (interpolated_pose.size() != resampled_arclength.size()) {
+    std::cerr << "[motion_utils]: Resampled pose size is different from resampled arclength"
+              << std::endl;
+    return input_path;
+  }
+
+  autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path;
+  resampled_path.header = input_path.header;
+  resampled_path.drivable_area = input_path.drivable_area;
+  resampled_path.points.resize(interpolated_pose.size());
+  for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+    autoware_auto_planning_msgs::msg::PathPoint path_point;
+    path_point.pose = interpolated_pose.at(i);
+    path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i);
+    path_point.lateral_velocity_mps = interpolated_v_lat.at(i);
+    path_point.heading_rate_rps = interpolated_heading_rate.at(i);
+    path_point.is_final = interpolated_is_final.at(i);
+    resampled_path.points.at(i).point = path_point;
+    resampled_path.points.at(i).lane_ids = interpolated_lane_ids.at(i);
+  }
+
+  return resampled_path;
+}
+
+autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path,
+  const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z,
+  const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point)
+{
+  // Check vector size and if out_arclength have the end point of the trajectory
+  if (input_path.points.size() < 2 || resample_interval < 1e-3) {
+    std::cerr << "[motion_utils]: input trajectory size or resample "
+                 "interval is invalid"
+              << std::endl;
+    return input_path;
+  }
+
+  // transform input_path
+  std::vector<autoware_auto_planning_msgs::msg::PathPoint> transformed_input_path(
+    input_path.points.size());
+  for (size_t i = 0; i < input_path.points.size(); ++i) {
+    transformed_input_path.at(i) = input_path.points.at(i).point;
+  }
+  // compute path length
+  const double input_path_len = motion_utils::calcArcLength(transformed_input_path);
+
+  std::vector<double> resampling_arclength;
+  for (double s = 0.0; s < input_path_len; s += resample_interval) {
+    resampling_arclength.push_back(s);
+  }
+  if (resampling_arclength.empty()) {
+    std::cerr << "[motion_utils]: resampling arclength is empty" << std::endl;
+    return input_path;
+  }
+
+  // Insert terminal point
+  if (input_path_len - resampling_arclength.back() < 1e-3) {
+    resampling_arclength.back() = input_path_len;
+  } else {
+    resampling_arclength.push_back(input_path_len);
+  }
+
+  // Insert stop point
+  if (resample_input_path_stop_point) {
+    const auto distance_to_stop_point =
+      motion_utils::calcDistanceToForwardStopPoint(transformed_input_path, 0);
+    if (distance_to_stop_point && !resampling_arclength.empty()) {
+      for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+        if (
+          resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
+          *distance_to_stop_point < resampling_arclength.at(i)) {
+          const double dist_to_prev_point =
+            std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
+          const double dist_to_following_point =
+            std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
+          if (dist_to_prev_point < 1e-3) {
+            resampling_arclength.at(i - 1) = *distance_to_stop_point;
+          } else if (dist_to_following_point < 1e-3) {
+            resampling_arclength.at(i) = *distance_to_stop_point;
+          } else {
+            resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  return resamplePath(
+    input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, use_zero_order_hold_for_v);
+}
+
 autoware_auto_planning_msgs::msg::Path resamplePath(
   const autoware_auto_planning_msgs::msg::Path & input_path,
   const std::vector<double> & resampled_arclength, const bool use_lerp_for_xy,
@@ -291,7 +456,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
 autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory,
   const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z,
-  const bool use_zero_order_hold_for_twist)
+  const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point)
 {
   const double input_trajectory_len = motion_utils::calcArcLength(input_trajectory.points);
   // Check vector size and if out_arclength have the end point of the trajectory
@@ -320,25 +485,27 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory(
   }
 
   // Insert stop point
-  const auto distance_to_stop_point =
-    motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0);
-  if (distance_to_stop_point && !resampling_arclength.empty()) {
-    for (size_t i = 1; i < resampling_arclength.size(); ++i) {
-      if (
-        resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
-        *distance_to_stop_point < resampling_arclength.at(i)) {
-        const double dist_to_prev_point =
-          std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
-        const double dist_to_following_point =
-          std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
-        if (dist_to_prev_point < 1e-3) {
-          resampling_arclength.at(i - 1) = *distance_to_stop_point;
-        } else if (dist_to_following_point < 1e-3) {
-          resampling_arclength.at(i) = *distance_to_stop_point;
-        } else {
-          resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+  if (resample_input_trajectory_stop_point) {
+    const auto distance_to_stop_point =
+      motion_utils::calcDistanceToForwardStopPoint(input_trajectory.points, 0);
+    if (distance_to_stop_point && !resampling_arclength.empty()) {
+      for (size_t i = 1; i < resampling_arclength.size(); ++i) {
+        if (
+          resampling_arclength.at(i - 1) <= *distance_to_stop_point &&
+          *distance_to_stop_point < resampling_arclength.at(i)) {
+          const double dist_to_prev_point =
+            std::fabs(*distance_to_stop_point - resampling_arclength.at(i - 1));
+          const double dist_to_following_point =
+            std::fabs(resampling_arclength.at(i) - *distance_to_stop_point);
+          if (dist_to_prev_point < 1e-3) {
+            resampling_arclength.at(i - 1) = *distance_to_stop_point;
+          } else if (dist_to_following_point < 1e-3) {
+            resampling_arclength.at(i) = *distance_to_stop_point;
+          } else {
+            resampling_arclength.insert(resampling_arclength.begin() + i, *distance_to_stop_point);
+          }
+          break;
         }
-        break;
       }
     }
   }
diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp
index a1acaf948f0d8..4b3c394e3b83e 100644
--- a/common/motion_utils/test/src/resample/test_resample.cpp
+++ b/common/motion_utils/test/src/resample/test_resample.cpp
@@ -27,6 +27,8 @@ namespace
 {
 using autoware_auto_planning_msgs::msg::Path;
 using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::createPoint;
@@ -44,6 +46,21 @@ geometry_msgs::msg::Pose createPose(
   return p;
 }
 
+PathPointWithLaneId generateTestPathPointWithLaneId(
+  const double x, const double y, const double z, const double theta = 0.0,
+  const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0,
+  const bool is_final = false, const std::vector<int64_t> & lane_ids = {})
+{
+  PathPointWithLaneId p;
+  p.point.pose = createPose(x, y, z, 0.0, 0.0, theta);
+  p.point.longitudinal_velocity_mps = vel_lon;
+  p.point.lateral_velocity_mps = vel_lat;
+  p.point.heading_rate_rps = heading_rate;
+  p.point.is_final = is_final;
+  p.lane_ids = lane_ids;
+  return p;
+}
+
 PathPoint generateTestPathPoint(
   const double x, const double y, const double z, const double theta = 0.0,
   const double vel_lon = 0.0, const double vel_lat = 0.0, const double heading_rate = 0.0)
@@ -70,6 +87,30 @@ TrajectoryPoint generateTestTrajectoryPoint(
   return p;
 }
 
+PathWithLaneId generateTestPathWithLaneId(
+  const size_t num_points, const double point_interval, const double vel_lon = 0.0,
+  const double vel_lat = 0.0, const double heading_rate_rps = 0.0, const double init_theta = 0.0,
+  const double delta_theta = 0.0)
+{
+  PathWithLaneId path;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double theta = init_theta + i * delta_theta;
+    const double x = i * point_interval * std::cos(theta);
+    const double y = i * point_interval * std::sin(theta);
+
+    PathPointWithLaneId p;
+    p.point.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
+    p.point.longitudinal_velocity_mps = vel_lon;
+    p.point.lateral_velocity_mps = vel_lat;
+    p.point.heading_rate_rps = heading_rate_rps;
+    p.point.is_final = (i == num_points - 1);
+    p.lane_ids = {static_cast<int64_t>(i)};
+    path.points.push_back(p);
+  }
+
+  return path;
+}
+
 template <class T>
 T generateTestPath(
   const size_t num_points, const double point_interval, const double vel_lon = 0.0,
@@ -132,6 +173,1317 @@ std::vector<double> generateArclength(const size_t num_points, const double inte
 }
 }  // namespace
 
+TEST(resample_path_with_lane_id, resample_path_by_vector)
+{
+  using motion_utils::resamplePath;
+  // Output is same as input
+  {
+    auto path = generateTestPathWithLaneId(10, 1.0, 3.0, 1.0, 0.01);
+    std::vector<double> resampled_arclength = generateArclength(10, 1.0);
+
+    {
+      const auto resampled_path = resamplePath(path, resampled_arclength);
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+
+    // Change the last point orientation
+    path.points.back() = generateTestPathPointWithLaneId(
+      9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9});
+    {
+      const auto resampled_path = resamplePath(path, resampled_arclength);
+      for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+
+      const auto p = resampled_path.points.back();
+      const auto ans_p = path.points.back();
+      const auto ans_quat = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
+      EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon);
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+      EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i));
+      }
+    }
+  }
+
+  // Output key is not same as input
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    std::vector<double> resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength);
+
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(4);
+      EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 7);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(5);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+    }
+  }
+
+  // No Interpolation
+  {
+    // Input path size is not enough for interpolation
+    {
+      autoware_auto_planning_msgs::msg::PathWithLaneId path;
+      path.points.resize(1);
+      for (size_t i = 0; i < 1; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {0});
+      }
+      std::vector<double> resampled_arclength = generateArclength(10, 1.0);
+
+      const auto resampled_path = resamplePath(path, resampled_arclength);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+
+    // Resampled Arclength size is not enough for interpolation
+    {
+      autoware_auto_planning_msgs::msg::PathWithLaneId path;
+      path.points.resize(10);
+      for (size_t i = 0; i < 10; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast<int64_t>(i)});
+      }
+      path.points.back().point.is_final = false;
+      std::vector<double> resampled_arclength = generateArclength(1, 1.0);
+
+      const auto resampled_path = resamplePath(path, resampled_arclength);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+
+    // Resampled Arclength is longer than input path
+    {
+      autoware_auto_planning_msgs::msg::PathWithLaneId path;
+      path.points.resize(10);
+      for (size_t i = 0; i < 10; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, true, {static_cast<int64_t>(i)});
+      }
+      path.points.back().point.is_final = false;
+      std::vector<double> resampled_arclength = generateArclength(3, 5.0);
+
+      const auto resampled_path = resamplePath(path, resampled_arclength);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+  }
+}
+
+TEST(resample_path_with_lane_id, resample_path_by_vector_backward)
+{
+  using motion_utils::resamplePath;
+
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, tier4_autoware_utils::pi, i * 1.0, i * 0.5, i * 0.1, false,
+        {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    std::vector<double> resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength);
+
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(4);
+      EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 7);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(5);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI);
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i).point;
+      EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // change initial orientation
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    path.points.at(0).point.pose.orientation =
+      tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0);
+    std::vector<double> resampled_arclength = {0.0, 1.2, 1.5, 5.3, 7.5, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength);
+
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 1.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.15, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(4);
+      EXPECT_NEAR(p.point.pose.position.x, 7.5, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 7.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 3.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.75, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 7);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(5);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    // Initial Orientation
+    {
+      const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI + M_PI / 3.0);
+      const auto p = resampled_path.points.at(0).point;
+      EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
+    }
+
+    const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(M_PI);
+    for (size_t i = 1; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i).point;
+      EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+}
+
+TEST(resample_path_with_lane_id, resample_path_by_vector_non_default)
+{
+  using motion_utils::resamplePath;
+
+  // Lerp x, y
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    std::vector<double> resampled_arclength = {0.0, 1.2, 5.3, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength, true);
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i).point;
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+    }
+  }
+
+  // Slerp z
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, i * 1.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    std::vector<double> resampled_arclength = {0.0, 1.2, 5.3, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength, false, false);
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 1.2, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 5.3, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 9.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    const double pitch = std::atan(1.0);
+    const auto ans_quat = tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, 0.0);
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i).point;
+      EXPECT_NEAR(p.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, ans_quat.w, epsilon);
+    }
+  }
+
+  // Lerp v
+  {
+    autoware_auto_planning_msgs::msg::PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+    std::vector<double> resampled_arclength = {0.0, 1.2, 5.3, 9.0};
+
+    const auto resampled_path = resamplePath(path, resampled_arclength, false, true, false);
+    {
+      const auto p = resampled_path.points.at(0);
+      EXPECT_NEAR(p.point.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 0);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1);
+      EXPECT_NEAR(p.point.pose.position.x, 1.2, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 1.2, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 0.6, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.12, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 1);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2);
+      EXPECT_NEAR(p.point.pose.position.x, 5.3, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 5.3, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 2.65, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.53, epsilon);
+      EXPECT_EQ(p.point.is_final, false);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 5);
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.longitudinal_velocity_mps, 9.0, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, 4.5, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, true);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), 9);
+      }
+    }
+
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i).point;
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+    }
+  }
+}
+
+TEST(resample_path_with_lane_id, resample_path_by_same_interval)
+{
+  using motion_utils::resamplePath;
+
+  // Same point resampling
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.back().point.is_final = true;
+
+    {
+      const auto resampled_path = resamplePath(path, 1.0);
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+    // Change the last point orientation
+    path.points.back() = generateTestPathPointWithLaneId(
+      9.0, 0.0, 0.0, tier4_autoware_utils::pi / 3.0, 3.0, 1.0, 0.01, true, {9});
+    {
+      const auto resampled_path = resamplePath(path, 1.0);
+      for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+
+      const auto p = resampled_path.points.back();
+      const auto ans_p = path.points.back();
+      const auto ans_quat = tier4_autoware_utils::createQuaternionFromYaw(0.0);
+      EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, ans_quat.x, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, ans_quat.y, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, ans_quat.z, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, ans_quat.w, epsilon);
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+      EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), ans_p.lane_ids.at(i));
+      }
+    }
+  }
+
+  // Normal Case without zero point
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.at(0).point.longitudinal_velocity_mps = 5.0;
+
+    const auto resampled_path = resamplePath(path, 0.1);
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i);
+      EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = i / 10;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon);
+      EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final);
+      for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+        EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j));
+      }
+    }
+  }
+
+  // Normal Case without stop point but with terminal point
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.at(0).point.longitudinal_velocity_mps = 5.0;
+    path.points.back().point.is_final = true;
+
+    const auto resampled_traj = resamplePath(path, 0.4);
+    for (size_t i = 0; i < resampled_traj.points.size() - 1; ++i) {
+      const auto p = resampled_traj.points.at(i);
+      EXPECT_NEAR(p.point.pose.position.x, 0.4 * i, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = i / 2.5;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.04 * i, epsilon);
+      EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final);
+      for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+        EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_traj.points.at(23);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = 9;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final);
+      for (size_t i = 0; i < p.lane_ids.size(); ++i) {
+        EXPECT_EQ(p.lane_ids.at(i), path.points.at(idx).lane_ids.at(i));
+      }
+    }
+  }
+
+  // Normal Case without stop point but with terminal point (Boundary Condition)
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.at(0).point.longitudinal_velocity_mps = 5.0;
+    path.points.back().point.is_final = true;
+
+    const auto resampled_path = resamplePath(path, 0.999);
+    for (size_t i = 0; i < resampled_path.points.size() - 1; ++i) {
+      const auto p = resampled_path.points.at(i);
+      EXPECT_NEAR(p.point.pose.position.x, 0.999 * i, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = i == 0 ? 0 : i - 1;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.0999 * i, epsilon);
+      EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final);
+      for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+        EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(10);
+      EXPECT_NEAR(p.point.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = 9;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.9, epsilon);
+      EXPECT_EQ(p.point.is_final, path.points.at(idx).point.is_final);
+      for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+        EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j));
+      }
+    }
+  }
+
+  // Normal Case with duplicated zero point
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.at(0).point.longitudinal_velocity_mps = 5.0;
+    path.points.at(5).point.longitudinal_velocity_mps = 0.0;
+    path.points.back().point.is_final = true;
+
+    const auto resampled_path = resamplePath(path, 0.1);
+    EXPECT_EQ(resampled_path.points.size(), static_cast<size_t>(91));
+    for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+      const auto p = resampled_path.points.at(i);
+      EXPECT_NEAR(p.point.pose.position.x, 0.1 * i, epsilon);
+      EXPECT_NEAR(p.point.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.point.pose.orientation.w, 1.0, epsilon);
+
+      const size_t idx = i / 10;
+      EXPECT_NEAR(
+        p.point.longitudinal_velocity_mps, path.points.at(idx).point.longitudinal_velocity_mps,
+        epsilon);
+      EXPECT_NEAR(
+        p.point.lateral_velocity_mps, path.points.at(idx).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.point.heading_rate_rps, 0.01 * i, epsilon);
+      for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+        EXPECT_EQ(p.lane_ids.at(j), path.points.at(idx).lane_ids.at(j));
+      }
+    }
+  }
+
+  // Normal Case with zero point
+  {
+    PathWithLaneId path;
+    path.points.resize(10);
+    for (size_t i = 0; i < 10; ++i) {
+      path.points.at(i) = generateTestPathPointWithLaneId(
+        i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+    }
+    path.points.at(0).point.longitudinal_velocity_mps = 8.0;
+    path.points.at(5).point.longitudinal_velocity_mps = 0.0;
+    path.points.back().point.is_final = true;
+
+    const auto resampled_path = resamplePath(path, 1.5);
+    EXPECT_EQ(resampled_path.points.size(), static_cast<size_t>(8));
+    {
+      const auto p = resampled_path.points.at(0).point;
+      const auto lane_ids = resampled_path.points.at(0).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(0).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(0).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.0, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(0).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(1).point;
+      const auto lane_ids = resampled_path.points.at(1).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 1.5, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(1).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(1).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.15, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(1).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(2).point;
+      const auto lane_ids = resampled_path.points.at(2).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 3.0, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(3).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(3).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.30, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(3).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(3).point;
+      const auto lane_ids = resampled_path.points.at(3).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 4.5, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(4).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(4).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.45, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(4).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(4).point;
+      const auto lane_ids = resampled_path.points.at(4).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 5.0, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(p.longitudinal_velocity_mps, 0.0, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(5).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.50, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(5).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(5).point;
+      const auto lane_ids = resampled_path.points.at(5).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 6.0, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(6).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(6).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.60, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(6).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(6).point;
+      const auto lane_ids = resampled_path.points.at(6).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 7.5, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(7).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(7).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.75, epsilon);
+      EXPECT_EQ(p.is_final, false);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(7).lane_ids.at(j));
+      }
+    }
+
+    {
+      const auto p = resampled_path.points.at(7).point;
+      const auto lane_ids = resampled_path.points.at(7).lane_ids;
+      EXPECT_NEAR(p.pose.position.x, 9.0, epsilon);
+      EXPECT_NEAR(p.pose.position.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.position.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.x, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.y, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
+      EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
+
+      EXPECT_NEAR(
+        p.longitudinal_velocity_mps, path.points.at(9).point.longitudinal_velocity_mps, epsilon);
+      EXPECT_NEAR(p.lateral_velocity_mps, path.points.at(9).point.lateral_velocity_mps, epsilon);
+      EXPECT_NEAR(p.heading_rate_rps, 0.90, epsilon);
+      EXPECT_EQ(p.is_final, true);
+      for (size_t j = 0; j < lane_ids.size(); ++j) {
+        EXPECT_EQ(lane_ids.at(j), path.points.at(9).lane_ids.at(j));
+      }
+    }
+  }
+
+  // No Resample
+  {
+    // Input path size is not enough for resample
+    {
+      PathWithLaneId path;
+      path.points.resize(1);
+      for (size_t i = 0; i < 1; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {0});
+      }
+
+      const auto resampled_path = resamplePath(path, 1.0);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+
+    // Resample interval is invalid
+    {
+      PathWithLaneId path;
+      path.points.resize(10);
+      for (size_t i = 0; i < 10; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+      }
+
+      const auto resampled_path = resamplePath(path, 1e-4);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+
+    // Resample interval is invalid (Negative value)
+    {
+      PathWithLaneId path;
+      path.points.resize(10);
+      for (size_t i = 0; i < 10; ++i) {
+        path.points.at(i) = generateTestPathPointWithLaneId(
+          i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1, false, {static_cast<int64_t>(i)});
+      }
+
+      const auto resampled_path = resamplePath(path, -5.0);
+      EXPECT_EQ(resampled_path.points.size(), path.points.size());
+      for (size_t i = 0; i < resampled_path.points.size(); ++i) {
+        const auto p = resampled_path.points.at(i);
+        const auto ans_p = path.points.at(i);
+        EXPECT_NEAR(p.point.pose.position.x, ans_p.point.pose.position.x, epsilon);
+        EXPECT_NEAR(p.point.pose.position.y, ans_p.point.pose.position.y, epsilon);
+        EXPECT_NEAR(p.point.pose.position.z, ans_p.point.pose.position.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.x, ans_p.point.pose.orientation.x, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.y, ans_p.point.pose.orientation.y, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.z, ans_p.point.pose.orientation.z, epsilon);
+        EXPECT_NEAR(p.point.pose.orientation.w, ans_p.point.pose.orientation.w, epsilon);
+        EXPECT_NEAR(
+          p.point.longitudinal_velocity_mps, ans_p.point.longitudinal_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.lateral_velocity_mps, ans_p.point.lateral_velocity_mps, epsilon);
+        EXPECT_NEAR(p.point.heading_rate_rps, ans_p.point.heading_rate_rps, epsilon);
+        EXPECT_EQ(p.point.is_final, ans_p.point.is_final);
+        for (size_t j = 0; j < p.lane_ids.size(); ++j) {
+          EXPECT_EQ(p.lane_ids.at(j), ans_p.lane_ids.at(j));
+        }
+      }
+    }
+  }
+}
+
 TEST(resample_path, resample_path_by_vector)
 {
   using motion_utils::resamplePath;
@@ -1369,7 +2721,6 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval)
       EXPECT_NEAR(p.pose.orientation.z, 0.0, epsilon);
       EXPECT_NEAR(p.pose.orientation.w, 1.0, epsilon);
 
-      std::cout << i << std::endl;
       const size_t idx = i == 0 ? 0 : i - 1;
       EXPECT_NEAR(
         p.longitudinal_velocity_mps, traj.points.at(idx).longitudinal_velocity_mps, epsilon);