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..d147a6d72 100644
--- a/trajectory_tracker/include/trajectory_tracker/path2d.h
+++ b/trajectory_tracker/include/trajectory_tracker/path2d.h
@@ -37,10 +37,11 @@
 #include <Eigen/Geometry>
 
 #include <geometry_msgs/Pose.h>
+#include <nav_msgs/Path.h>
 #include <tf2/utils.h>
 
-#include <trajectory_tracker/eigen_line.h>
 #include <trajectory_tracker/average.h>
+#include <trajectory_tracker/eigen_line.h>
 
 namespace trajectory_tracker
 {
@@ -132,10 +133,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 +180,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 +260,55 @@ class Path2D : public std::vector<Pose2D>
     }
     return curv;
   }
+  inline void fromMsg(const nav_msgs::Path& 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.pose, 0.0);
+      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);
+    }
+  }
+  inline void toMsg(nav_msgs::Path& path) const
+  {
+    path.poses.clear();
+    for (const auto& pose : *this)
+    {
+      geometry_msgs::PoseStamped pose_msg;
+      pose_msg.header = path.header;
+      pose_msg.pose.position.x = pose.pos_.x();
+      pose_msg.pose.position.y = pose.pos_.y();
+      pose_msg.pose.orientation.z = std::sin(pose.yaw_ / 2);
+      pose_msg.pose.orientation.w = std::cos(pose.yaw_ / 2);
+      path.poses.push_back(pose_msg);
+    }
+  }
 };
 }  // namespace trajectory_tracker
 
diff --git a/trajectory_tracker/test/src/test_path2d.cpp b/trajectory_tracker/test/src/test_path2d.cpp
index 22fe1789e..1d92c68d0 100644
--- a/trajectory_tracker/test/src/test_path2d.cpp
+++ b/trajectory_tracker/test/src/test_path2d.cpp
@@ -29,6 +29,7 @@
 
 #include <algorithm>
 #include <cstddef>
+#include <limits>
 #include <string>
 
 #include <gtest/gtest.h>
@@ -36,6 +37,8 @@
 #include <trajectory_tracker/eigen_line.h>
 #include <trajectory_tracker/path2d.h>
 
+#include <ros/ros.h>
+
 namespace
 {
 double getRemainedDistance(const trajectory_tracker::Path2D& path, const Eigen::Vector2d& p)
@@ -144,6 +147,115 @@ 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);
+  ASSERT_EQ(path.size(), 6);
+
+  for (size_t i = 0; i < path.size(); ++i)
+  {
+    size_t org_idx = i;
+    if (i == 5)
+    {
+      org_idx += 2;
+    }
+    else if (i >= 2)
+    {
+      org_idx += 1;
+    }
+    EXPECT_EQ(path[i].pos_.x(), path_msg_org.poses[org_idx].pose.position.x) << "idx: " << i << " org_idx: " << org_idx;
+    EXPECT_EQ(path[i].pos_.y(), path_msg_org.poses[org_idx].pose.position.y) << "idx: " << i << " org_idx: " << org_idx;
+    EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg_org.poses[org_idx].pose.orientation), 1.0e-6)
+        << "idx: " << i << " org_idx: " << org_idx;
+  }
+
+  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) << "idx: " << i;
+    EXPECT_EQ(path[i].pos_.y(), path_msg.poses[i].pose.position.y) << "idx: " << i;
+    EXPECT_NEAR(path[i].yaw_, tf2::getYaw(path_msg.poses[i].pose.orientation), 1.0e-6) << "idx: " << 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);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);