diff --git a/trajectory_tracker/CMakeLists.txt b/trajectory_tracker/CMakeLists.txt
index ab621dfde..0a99a8a22 100644
--- a/trajectory_tracker/CMakeLists.txt
+++ b/trajectory_tracker/CMakeLists.txt
@@ -26,6 +26,7 @@ generate_dynamic_reconfigure_options(
 )
 
 catkin_package(
+  INCLUDE_DIRS include
   CATKIN_DEPENDS ${CATKIN_DEPENDS}
 )
 
@@ -72,3 +73,7 @@ install(TARGETS
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
diff --git a/trajectory_tracker/include/trajectory_tracker/path2d.h b/trajectory_tracker/include/trajectory_tracker/path2d.h
index 6c53def26..9e90a34ba 100644
--- a/trajectory_tracker/include/trajectory_tracker/path2d.h
+++ b/trajectory_tracker/include/trajectory_tracker/path2d.h
@@ -30,17 +30,20 @@
 #ifndef TRAJECTORY_TRACKER_PATH2D_H
 #define TRAJECTORY_TRACKER_PATH2D_H
 
-#include <vector>
 #include <limits>
+#include <utility>
+#include <vector>
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
 #include <geometry_msgs/Pose.h>
+#include <nav_msgs/Path.h>
 #include <tf2/utils.h>
+#include <trajectory_tracker_msgs/PathWithVelocity.h>
 
-#include <trajectory_tracker/eigen_line.h>
 #include <trajectory_tracker/average.h>
+#include <trajectory_tracker/eigen_line.h>
 
 namespace trajectory_tracker
 {
@@ -63,12 +66,24 @@ class Pose2D
     , velocity_(velocity)
   {
   }
-  inline explicit Pose2D(const geometry_msgs::Pose& pose, float velocity)
+  inline Pose2D(const geometry_msgs::Pose& pose, float velocity)
     : pos_(Eigen::Vector2d(pose.position.x, pose.position.y))
     , yaw_(tf2::getYaw(pose.orientation))
     , velocity_(velocity)
   {
   }
+  inline explicit Pose2D(const geometry_msgs::PoseStamped& pose)
+    : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y))
+    , yaw_(tf2::getYaw(pose.pose.orientation))
+    , velocity_(std::numeric_limits<float>::quiet_NaN())
+  {
+  }
+  inline explicit Pose2D(const trajectory_tracker_msgs::PoseStampedWithVelocity& pose)
+    : pos_(Eigen::Vector2d(pose.pose.position.x, pose.pose.position.y))
+    , yaw_(tf2::getYaw(pose.pose.orientation))
+    , velocity_(pose.linear_velocity.x)
+  {
+  }
   inline void rotate(const float ang)
   {
     const float org_x = pos_.x();
@@ -84,6 +99,19 @@ class Pose2D
     while (yaw_ > 2 * M_PI)
       yaw_ -= 2 * M_PI;
   }
+  void toMsg(geometry_msgs::PoseStamped& pose) const
+  {
+    pose.pose.position.x = pos_.x();
+    pose.pose.position.y = pos_.y();
+    pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), yaw_));
+  }
+  void toMsg(trajectory_tracker_msgs::PoseStampedWithVelocity& pose) const
+  {
+    pose.pose.position.x = pos_.x();
+    pose.pose.position.y = pos_.y();
+    pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), yaw_));
+    pose.linear_velocity.x = velocity_;
+  }
 };
 class Path2D : public std::vector<Pose2D>
 {
@@ -132,10 +160,24 @@ class Path2D : public std::vector<Pose2D>
       const Eigen::Vector2d& target,
       const float max_search_range = 0,
       const float epsilon = 1e-6) const
+  {
+    return findNearestWithDistance(begin, end, target, max_search_range, epsilon).first;
+  }
+  inline std::pair<ConstIterator, double> findNearestWithDistance(
+      const ConstIterator& begin,
+      const ConstIterator& end,
+      const Eigen::Vector2d& target,
+      const float max_search_range = 0,
+      const float epsilon = 1e-6) const
   {
     if (begin == end)
-      return end;
-
+    {
+      if (end == this->end())
+      {
+        return std::make_pair(end, std::numeric_limits<double>::max());
+      }
+      return std::make_pair(end, (end->pos_ - target).norm());
+    }
     float distance_path_search = 0;
     ConstIterator it_nearest = begin;
     float min_dist = (begin->pos_ - target).norm() + epsilon;
@@ -165,7 +207,7 @@ class Path2D : public std::vector<Pose2D>
       }
       it_prev = it;
     }
-    return it_nearest;
+    return std::make_pair(it_nearest, min_dist);
   }
   inline double remainedDistance(
       const ConstIterator& begin,
@@ -245,6 +287,54 @@ class Path2D : public std::vector<Pose2D>
     }
     return curv;
   }
+  // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path
+  template <typename PATH_TYPE>
+  inline void fromMsg(const PATH_TYPE& path, const double in_place_turn_eps = 1.0e-6)
+  {
+    clear();
+    bool in_place_turning = false;
+    trajectory_tracker::Pose2D in_place_turn_end;
+    for (const auto& pose : path.poses)
+    {
+      const trajectory_tracker::Pose2D next(pose);
+      if (empty())
+      {
+        push_back(next);
+        continue;
+      }
+      if ((back().pos_ - next.pos_).squaredNorm() >= std::pow(in_place_turn_eps, 2))
+      {
+        if (in_place_turning)
+        {
+          push_back(in_place_turn_end);
+          in_place_turning = false;
+        }
+        push_back(next);
+      }
+      else
+      {
+        in_place_turn_end = trajectory_tracker::Pose2D(
+            back().pos_, next.yaw_, next.velocity_);
+        in_place_turning = true;
+      }
+    }
+    if (in_place_turning)
+    {
+      push_back(in_place_turn_end);
+    }
+  }
+  // PATH_TYPE should be trajectory_tracker_msgs::PathWithVelocity or nav_msgs::Path
+  template <typename PATH_TYPE>
+  inline void toMsg(PATH_TYPE& path) const
+  {
+    path.poses.clear();
+    path.poses.resize(size());
+    for (size_t i = 0; i < size(); ++i)
+    {
+      path.poses[i].header = path.header;
+      at(i).toMsg(path.poses[i]);
+    }
+  }
 };
 }  // namespace trajectory_tracker
 
diff --git a/trajectory_tracker/src/trajectory_tracker.cpp b/trajectory_tracker/src/trajectory_tracker.cpp
index bb6124737..b7df6d834 100644
--- a/trajectory_tracker/src/trajectory_tracker.cpp
+++ b/trajectory_tracker/src/trajectory_tracker.cpp
@@ -282,62 +282,22 @@ void TrackerNode::cbSpeed(const std_msgs::Float32::ConstPtr& msg)
   vel_[0] = msg->data;
 }
 
-namespace
-{
-float getVelocity(const geometry_msgs::PoseStamped& msg)
-{
-  return std::numeric_limits<float>::quiet_NaN();
-}
-float getVelocity(const trajectory_tracker_msgs::PoseStampedWithVelocity& msg)
-{
-  return msg.linear_velocity.x;
-}
-}  // namespace
-
 template <typename MSG_TYPE>
 void TrackerNode::cbPath(const typename MSG_TYPE::ConstPtr& msg)
 {
   path_header_ = msg->header;
-  path_.clear();
   is_path_updated_ = true;
   path_step_done_ = 0;
-  if (msg->poses.size() == 0)
-    return;
-
-  trajectory_tracker::Pose2D in_place_turn_end;
-  bool in_place_turning = false;
-
-  auto j = msg->poses.begin();
-  path_.push_back(trajectory_tracker::Pose2D(j->pose, getVelocity(*j)));
-  for (++j; j < msg->poses.end(); ++j)
+  path_.fromMsg(*msg, epsilon_);
+  for (const auto& path_pose : path_)
   {
-    const float velocity = getVelocity(*j);
-    if (std::isfinite(velocity) && velocity < -0.0)
+    if (std::isfinite(path_pose.velocity_) && path_pose.velocity_ < -0.0)
     {
       ROS_ERROR_THROTTLE(1.0, "path_velocity.velocity.x must be positive");
       path_.clear();
       return;
     }
-    const trajectory_tracker::Pose2D next(j->pose, velocity);
-
-    if ((path_.back().pos_ - next.pos_).squaredNorm() >= std::pow(epsilon_, 2))
-    {
-      if (in_place_turning)
-      {
-        path_.push_back(in_place_turn_end);
-        in_place_turning = false;
-      }
-      path_.push_back(next);
-    }
-    else
-    {
-      in_place_turn_end = trajectory_tracker::Pose2D(
-          path_.back().pos_, next.yaw_, next.velocity_);
-      in_place_turning = true;
-    }
   }
-  if (in_place_turning)
-    path_.push_back(in_place_turn_end);
 }
 
 void TrackerNode::cbOdometry(const nav_msgs::Odometry::ConstPtr& odom)
diff --git a/trajectory_tracker/test/src/test_path2d.cpp b/trajectory_tracker/test/src/test_path2d.cpp
index 22fe1789e..411f463c6 100644
--- a/trajectory_tracker/test/src/test_path2d.cpp
+++ b/trajectory_tracker/test/src/test_path2d.cpp
@@ -28,13 +28,18 @@
  */
 
 #include <algorithm>
+#include <cmath>
 #include <cstddef>
+#include <limits>
 #include <string>
+#include <vector>
 
 #include <gtest/gtest.h>
 
+#include <nav_msgs/Path.h>
 #include <trajectory_tracker/eigen_line.h>
 #include <trajectory_tracker/path2d.h>
+#include <trajectory_tracker_msgs/PathWithVelocity.h>
 
 namespace
 {
@@ -144,6 +149,148 @@ TEST(Path2D, LocalGoalWithSwitchBack)
   }
 }
 
+TEST(Path2D, FindNearestWithDistance)
+{
+  trajectory_tracker::Path2D path;
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.5, 0.5), 0, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.6, 0.5), 0, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.7, 0.5), 0, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.8, 0.6), M_PI / 4, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.7), M_PI / 4, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.8), M_PI / 2, 1));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.9, 0.9), M_PI / 2, 1));
+
+  {
+    // The nearest line is (0.8, 0.6) - (0.9, 0.7), and the nearest point on the line is (0.85, 0.65).
+    const auto nearest_with_dist = path.findNearestWithDistance(path.begin(), path.end(), Eigen::Vector2d(1.0, 0.5));
+    EXPECT_EQ(nearest_with_dist.first, path.begin() + 4);
+    EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.85, 2) + std::pow(0.5 - 0.65, 2)), 1.0e-6);
+  }
+  {
+    // The nearest point is (0.7, 0.5).
+    const auto nearest_with_dist =
+        path.findNearestWithDistance(path.begin(), path.begin() + 3, Eigen::Vector2d(1.0, 0.5));
+    EXPECT_EQ(nearest_with_dist.first, path.begin() + 2);
+    EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.7, 2) + std::pow(0.5 - 0.5, 2)), 1.0e-6);
+  }
+  {
+    // Test edge cases
+    const auto nearest_with_dist =
+        path.findNearestWithDistance(path.begin() + 5, path.begin() + 5, Eigen::Vector2d(1.0, 0.5));
+    EXPECT_EQ(nearest_with_dist.first, path.begin() + 5);
+    EXPECT_NEAR(nearest_with_dist.second, std::sqrt(std::pow(1.0 - 0.9, 2) + std::pow(0.5 - 0.8, 2)), 1.0e-6);
+
+    const auto invalid_result =
+        path.findNearestWithDistance(path.end(), path.end(), Eigen::Vector2d(1.0, 0.5));
+    EXPECT_EQ(invalid_result.first, path.end());
+    EXPECT_EQ(invalid_result.second, std::numeric_limits<double>::max());
+  }
+}
+
+TEST(Path2D, Conversions)
+{
+  nav_msgs::Path path_msg_org;
+  path_msg_org.poses.resize(8);
+  path_msg_org.poses[0].pose.position.x = 0.0;
+  path_msg_org.poses[0].pose.position.y = 0.0;
+  path_msg_org.poses[0].pose.orientation.w = 1.0;
+  path_msg_org.poses[1].pose.position.x = 1.0;  // Start of in-place turning
+  path_msg_org.poses[1].pose.position.y = 0.0;
+  path_msg_org.poses[1].pose.orientation.w = 1.0;
+  path_msg_org.poses[2].pose.position.x = 1.0;
+  path_msg_org.poses[2].pose.position.y = 0.0;
+  path_msg_org.poses[2].pose.orientation.z = std::sin(M_PI / 8);
+  path_msg_org.poses[2].pose.orientation.w = std::cos(M_PI / 8);
+  path_msg_org.poses[3].pose.position.x = 1.0;  // End of in-place turning
+  path_msg_org.poses[3].pose.position.y = 0.0;
+  path_msg_org.poses[3].pose.orientation.z = std::sin(M_PI / 4);
+  path_msg_org.poses[3].pose.orientation.w = std::cos(M_PI / 4);
+  path_msg_org.poses[4].pose.position.x = 1.0;
+  path_msg_org.poses[4].pose.position.y = 1.0;
+  path_msg_org.poses[4].pose.orientation.z = std::sin(M_PI / 4);
+  path_msg_org.poses[4].pose.orientation.w = std::cos(M_PI / 4);
+  path_msg_org.poses[5].pose.position.x = 1.0;  // Start of in-place turning
+  path_msg_org.poses[5].pose.position.y = 2.0;
+  path_msg_org.poses[5].pose.orientation.z = std::sin(M_PI / 4);
+  path_msg_org.poses[5].pose.orientation.w = std::cos(M_PI / 4);
+  path_msg_org.poses[6].pose.position.x = 1.0;
+  path_msg_org.poses[6].pose.position.y = 2.0;
+  path_msg_org.poses[6].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 6);
+  path_msg_org.poses[6].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 6);
+  path_msg_org.poses[7].pose.position.x = 1.0;  // End of in-place turning
+  path_msg_org.poses[7].pose.position.y = 2.0;
+  path_msg_org.poses[7].pose.orientation.z = std::sin(M_PI / 4 + M_PI / 3);
+  path_msg_org.poses[7].pose.orientation.w = std::cos(M_PI / 4 + M_PI / 3);
+
+  trajectory_tracker::Path2D path;
+  path.fromMsg(path_msg_org);
+
+  const std::vector<size_t> expected_org_indexes = {0, 1, 3, 4, 5, 7};
+  ASSERT_EQ(path.size(), 6);
+  for (size_t i = 0; i < path.size(); ++i)
+  {
+    const size_t org_index = expected_org_indexes[i];
+    EXPECT_EQ(path[i].pos_.x(), path_msg_org.poses[org_index].pose.position.x) << "i: " << i << " org: " << org_index;
+    EXPECT_EQ(path[i].pos_.y(), path_msg_org.poses[org_index].pose.position.y) << "i: " << i << " org: " << org_index;
+    EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg_org.poses[org_index].pose.orientation), 1.0e-6)
+        << "i: " << i << " org: " << org_index;
+    EXPECT_TRUE(std::isnan(path[i].velocity_));
+  }
+
+  nav_msgs::Path path_msg;
+  path_msg.header.frame_id = "map";
+  path_msg.header.stamp = ros::Time(123.456);
+  path.toMsg(path_msg);
+  ASSERT_EQ(path_msg.poses.size(), 6);
+  for (size_t i = 0; i < path.size(); ++i)
+  {
+    EXPECT_EQ(path[i].pos_.x(), path_msg.poses[i].pose.position.x) << "i: " << i;
+    EXPECT_EQ(path[i].pos_.y(), path_msg.poses[i].pose.position.y) << "i: " << i;
+    EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg.poses[i].pose.orientation), 1.0e-6) << "i: " << i;
+    EXPECT_EQ(path_msg.poses[i].header.frame_id, path_msg.header.frame_id);
+    EXPECT_EQ(path_msg.poses[i].header.stamp, path_msg.header.stamp);
+  }
+
+  trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg_org;
+  path_with_vel_msg_org.poses.resize(path_msg_org.poses.size());
+  for (size_t i = 0; i < path_msg_org.poses.size(); ++i)
+  {
+    path_with_vel_msg_org.poses[i].pose = path_msg_org.poses[i].pose;
+    path_with_vel_msg_org.poses[i].linear_velocity.x = i * 0.1;
+  }
+
+  trajectory_tracker::Path2D path_with_vel;
+  path_with_vel.fromMsg(path_with_vel_msg_org);
+
+  ASSERT_EQ(path_with_vel.size(), 6);
+  for (size_t i = 0; i < path_with_vel.size(); ++i)
+  {
+    const size_t org_index = expected_org_indexes[i];
+    EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg_org.poses[org_index].pose.position.x)
+        << "i: " << i << " org: " << org_index;
+    EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg_org.poses[org_index].pose.position.y)
+        << "i: " << i << " org: " << org_index;
+    EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg_org.poses[org_index].pose.orientation), 1.0e-6)
+        << "i: " << i << " org: " << org_index;
+    EXPECT_NEAR(path_with_vel[i].velocity_, org_index * 0.1, 1.0e-6) << "i: " << i << " org: " << org_index;
+  }
+
+  trajectory_tracker_msgs::PathWithVelocity path_with_vel_msg;
+  path_with_vel_msg.header.frame_id = "map";
+  path_with_vel_msg.header.stamp = ros::Time(123.456);
+  path_with_vel.toMsg(path_with_vel_msg);
+  ASSERT_EQ(path_with_vel_msg.poses.size(), 6);
+  for (size_t i = 0; i < path_with_vel.size(); ++i)
+  {
+    EXPECT_EQ(path_with_vel[i].pos_.x(), path_with_vel_msg.poses[i].pose.position.x) << "i: " << i;
+    EXPECT_EQ(path_with_vel[i].pos_.y(), path_with_vel_msg.poses[i].pose.position.y) << "i: " << i;
+    EXPECT_NEAR(path_with_vel[i].yaw_, tf2::getYaw(path_with_vel_msg.poses[i].pose.orientation), 1.0e-6)
+        << "i: " << i;
+    EXPECT_EQ(path_with_vel_msg.poses[i].header.frame_id, path_with_vel_msg.header.frame_id);
+    EXPECT_EQ(path_with_vel_msg.poses[i].header.stamp, path_with_vel_msg.header.stamp);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);