diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
index 178385f2ef9ef..55755a7f3e9ba 100644
--- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
+++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp
@@ -183,136 +183,6 @@ TEST(TestLongitudinalControllerUtils, calcElevationAngle)
   EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4);
 }
 
-// TEST(TestLongitudinalControllerUtils, calcDistanceAfterTimeDelay)
-//{
-//   using geometry_msgs::msg::Pose;
-//   const double abs_err = 1e-7;
-//   Pose current_pose;
-//   current_pose.position.x = 0.0;
-//   current_pose.position.y = 0.0;
-//   current_pose.position.z = 0.0;
-//   tf2::Quaternion quaternion_tf;
-//   quaternion_tf.setRPY(0.0, 0.0, 0.0);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//
-//   // With a delay acceleration and/or a velocity of 0.0 there is no change of position
-//   double delay_time = 0.0;
-//   double current_vel = 0.0;
-//   double current_acc = 0.0;
-//   Pose delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   delay_time = 1.0;
-//   current_vel = 0.0;
-//   current_acc = 0.0;
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   delay_time = 0.0;
-//   current_vel = 1.0;
-//   current_acc = 0.0;
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   // With both delay and velocity: change of position
-//   delay_time = 1.0;
-//   current_vel = 1.0;
-//   current_acc = 0.0;
-//
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x + current_vel * delay_time,
-//   abs_err); EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   // With all, acceleration, delay and velocity: change of position
-//   delay_time = 1.0;
-//   current_vel = 1.0;
-//   current_acc = 1.0;
-//
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(
-//     delayed_pose.position.x,
-//     current_pose.position.x + current_vel * delay_time +
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   // Vary the yaw
-//   quaternion_tf.setRPY(0.0, 0.0, M_PI);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(
-//     delayed_pose.position.x,
-//     current_pose.position.x - current_vel * delay_time -
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   quaternion_tf.setRPY(0.0, 0.0, M_PI_2);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-//   EXPECT_NEAR(
-//     delayed_pose.position.y,
-//     current_pose.position.y + current_vel * delay_time +
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   quaternion_tf.setRPY(0.0, 0.0, -M_PI_2);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(delayed_pose.position.x, current_pose.position.x, abs_err);
-//   EXPECT_NEAR(
-//     delayed_pose.position.y,
-//     current_pose.position.y - current_vel * delay_time -
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   // Vary the pitch : no effect /!\ NOTE: bug with roll of +-PI/2 which rotates the yaw by PI
-//   quaternion_tf.setRPY(0.0, M_PI_4, 0.0);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(
-//     delayed_pose.position.x,
-//     current_pose.position.x + current_vel * delay_time +
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-//
-//   // Vary the roll : no effect
-//   quaternion_tf.setRPY(M_PI_2, 0.0, 0.0);
-//   current_pose.orientation = tf2::toMsg(quaternion_tf);
-//   delayed_pose = longitudinal_utils::calcDistanceAfterTimeDelay(
-//     current_pose, delay_time, current_vel, current_acc);
-//   EXPECT_NEAR(
-//     delayed_pose.position.x,
-//     current_pose.position.x + current_vel * delay_time +
-//       0.5 * current_acc * delay_time * delay_time,
-//     abs_err);
-//   EXPECT_NEAR(delayed_pose.position.y, current_pose.position.y, abs_err);
-//   EXPECT_NEAR(delayed_pose.position.z, current_pose.position.z, abs_err);
-// }
-
 TEST(TestLongitudinalControllerUtils, lerpOrientation)
 {
   geometry_msgs::msg::Quaternion result;
@@ -369,105 +239,123 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
   EXPECT_DOUBLE_EQ(yaw, M_PI_4 / 2);
 }
 
-// TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
-//{
-//   using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-//   using geometry_msgs::msg::Pose;
-//   const double abs_err = 1e-15;
-//   decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points;
-//   TrajectoryPoint p;
-//   p.pose.position.x = 0.0;
-//   p.pose.position.y = 0.0;
-//   p.longitudinal_velocity_mps = 10.0;
-//   p.acceleration_mps2 = 10.0;
-//   points.push_back(p);
-//   p.pose.position.x = 1.0;
-//   p.pose.position.y = 0.0;
-//   p.longitudinal_velocity_mps = 20.0;
-//   p.acceleration_mps2 = 20.0;
-//   points.push_back(p);
-//   p.pose.position.x = 1.0;
-//   p.pose.position.y = 1.0;
-//   p.longitudinal_velocity_mps = 30.0;
-//   p.acceleration_mps2 = 30.0;
-//   points.push_back(p);
-//   p.pose.position.x = 2.0;
-//   p.pose.position.y = 1.0;
-//   p.longitudinal_velocity_mps = 40.0;
-//   p.acceleration_mps2 = 40.0;
-//   points.push_back(p);
-//   TrajectoryPoint result;
-//   Pose pose;
-//   double max_dist = 3.0;
-//   double max_yaw = 0.7;
-//   // Points on the trajectory gives back the original trajectory points values
-//   pose.position.x = 0.0;
-//   pose.position.y = 0.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 10.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 10.0, abs_err);
-//
-//   pose.position.x = 1.0;
-//   pose.position.y = 0.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 20.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 20.0, abs_err);
-//
-//   pose.position.x = 1.0;
-//   pose.position.y = 1.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 30.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 30.0, abs_err);
-//
-//   pose.position.x = 2.0;
-//   pose.position.y = 1.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 40.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 40.0, abs_err);
-//
-//   // Interpolate between trajectory points
-//   pose.position.x = 0.5;
-//   pose.position.y = 0.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
-//   pose.position.x = 0.75;
-//   pose.position.y = 0.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, pose.position.y, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 17.5, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 17.5, abs_err);
-//
-//   // Interpolate away from the trajectory (interpolated point is projected)
-//   pose.position.x = 0.5;
-//   pose.position.y = -1.0;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
-//
-//   // Ambiguous projections: possibility with the lowest index is used
-//   pose.position.x = 0.5;
-//   pose.position.y = 0.5;
-//   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
-//   EXPECT_NEAR(result.pose.position.x, pose.position.x, abs_err);
-//   EXPECT_NEAR(result.pose.position.y, 0.0, abs_err);
-//   EXPECT_NEAR(result.longitudinal_velocity_mps, 15.0, abs_err);
-//   EXPECT_NEAR(result.acceleration_mps2, 15.0, abs_err);
-// }
+ TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint)
+{
+   using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+   using geometry_msgs::msg::Pose;
+   const double abs_err = 1e-15;
+   decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points;
+   TrajectoryPoint p;
+   p.pose.position.x = 0.0;
+   p.pose.position.y = 0.0;
+   p.pose.position.z = 0.0;
+   p.longitudinal_velocity_mps = 10.0;
+   p.acceleration_mps2 = 10.0;
+   points.push_back(p);
+   p.pose.position.x = 1.0;
+   p.pose.position.y = 0.0;
+   p.pose.position.z = 0.0;
+   p.longitudinal_velocity_mps = 20.0;
+   p.acceleration_mps2 = 20.0;
+   points.push_back(p);
+   p.pose.position.x = 1.0;
+   p.pose.position.y = 1.0;
+   p.pose.position.z = 1.0;
+   p.longitudinal_velocity_mps = 30.0;
+   p.acceleration_mps2 = 30.0;
+   points.push_back(p);
+   p.pose.position.x = 2.0;
+   p.pose.position.y = 1.0;
+   p.pose.position.z = 2.0;
+   p.longitudinal_velocity_mps = 40.0;
+   p.acceleration_mps2 = 40.0;
+   points.push_back(p);
+   Pose pose;
+   double max_dist = 3.0;
+   double max_yaw = 0.7;
+   // Points on the trajectory gives back the original trajectory points values
+   pose.position.x = 0.0;
+   pose.position.y = 0.0;
+   pose.position.z = 0.0;
+
+   auto result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 10.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 10.0, abs_err);
+
+   pose.position.x = 1.0;
+   pose.position.y = 0.0;
+   pose.position.z = 0.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 20.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 20.0, abs_err);
+
+   pose.position.x = 1.0;
+   pose.position.y = 1.0;
+   pose.position.z = 1.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 30.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 30.0, abs_err);
+
+   pose.position.x = 2.0;
+   pose.position.y = 1.0;
+   pose.position.z = 2.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 40.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 40.0, abs_err);
+
+   // Interpolate between trajectory points
+   pose.position.x = 0.5;
+   pose.position.y = 0.0;
+   pose.position.z = 0.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);
+   pose.position.x = 0.75;
+   pose.position.y = 0.0;
+   pose.position.z = 0.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, pose.position.y, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 17.5, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 17.5, abs_err);
+
+   // Interpolate away from the trajectory (interpolated point is projected)
+   pose.position.x = 0.5;
+   pose.position.y = -1.0;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);
+
+   // Ambiguous projections: possibility with the lowest index is used
+   pose.position.x = 0.5;
+   pose.position.y = 0.5;
+   result = longitudinal_utils::lerpTrajectoryPoint(points, pose, max_dist, max_yaw);
+   EXPECT_NEAR(result.first.pose.position.x, pose.position.x, abs_err);
+   EXPECT_NEAR(result.first.pose.position.y, 0.0, abs_err);
+   EXPECT_NEAR(result.first.pose.position.z, pose.position.z, abs_err);
+   EXPECT_NEAR(result.first.longitudinal_velocity_mps, 15.0, abs_err);
+   EXPECT_NEAR(result.first.acceleration_mps2, 15.0, abs_err);
+ }
 
 TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter)
 {