diff --git a/planner_cspace/CMakeLists.txt b/planner_cspace/CMakeLists.txt
index 50af3512..5ef773b6 100644
--- a/planner_cspace/CMakeLists.txt
+++ b/planner_cspace/CMakeLists.txt
@@ -22,6 +22,7 @@ set(CATKIN_DEPENDS
   neonavigation_common
   neonavigation_metrics_msgs
   planner_cspace_msgs
+  trajectory_tracker
   trajectory_tracker_msgs
 )
 
@@ -55,6 +56,7 @@ add_executable(planner_3d
   src/planner_3d.cpp
   src/distance_map.cpp
   src/rotation_cache.cpp
+  src/start_pose_predictor.cpp
 )
 target_link_libraries(planner_3d ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
 add_dependencies(planner_3d ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
diff --git a/planner_cspace/cfg/Planner3D.cfg b/planner_cspace/cfg/Planner3D.cfg
index 5c90e9d5..47b7727f 100755
--- a/planner_cspace/cfg/Planner3D.cfg
+++ b/planner_cspace/cfg/Planner3D.cfg
@@ -46,5 +46,7 @@ gen.add("force_goal_orientation", bool_t, 0, "", True)
 gen.add("temporary_escape", bool_t, 0, "", True)
 gen.add("fast_map_update", bool_t, 0, "", False)
 gen.add("max_retry_num", int_t, 0, "", -1, -1, 100)
+gen.add("keep_a_part_of_previous_path", bool_t, 0, "If true, a part of the previous path is preserved to avoid radical path changes.", False)
+gen.add("dist_stop_to_previous_path", double_t, 0, "Valid only when keep_a_part_of_previous_path is true. This should be the same as dist_stop parameter of trajectory_tracker.", 0.1, 0.0, 1.0)
 
 exit(gen.generate(PACKAGE, "planner_cspace", "Planner3D"))
diff --git a/planner_cspace/include/planner_cspace/planner_3d/grid_metric_converter.h b/planner_cspace/include/planner_cspace/planner_3d/grid_metric_converter.h
index 76a2ab1f..659a3f94 100644
--- a/planner_cspace/include/planner_cspace/planner_3d/grid_metric_converter.h
+++ b/planner_cspace/include/planner_cspace/planner_3d/grid_metric_converter.h
@@ -79,7 +79,7 @@ inline void metric2Grid(
 }
 
 template <template <class, class> class STL_CONTAINER = std::list>
-void grid2MetricPath(
+void appendGridPath2MetricPath(
     const costmap_cspace_msgs::MapMetaData3D& map_info,
     const STL_CONTAINER<CyclicVecFloat<3, 2>,
                         std::allocator<CyclicVecFloat<3, 2>>>& path_grid,
diff --git a/planner_cspace/include/planner_cspace/planner_3d/start_pose_predictor.h b/planner_cspace/include/planner_cspace/planner_3d/start_pose_predictor.h
new file mode 100644
index 00000000..5367611b
--- /dev/null
+++ b/planner_cspace/include/planner_cspace/planner_3d/start_pose_predictor.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2023, the neonavigation authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PLANNER_CSPACE_PLANNER_3D_START_POSE_PREDICTOR_H
+#define PLANNER_CSPACE_PLANNER_3D_START_POSE_PREDICTOR_H
+
+#include <vector>
+
+#include <nav_msgs/Path.h>
+
+#include <costmap_cspace_msgs/CSpace3D.h>
+#include <planner_cspace/grid_astar.h>
+#include <planner_cspace/planner_3d/grid_metric_converter.h>
+#include <trajectory_tracker/path2d.h>
+
+namespace planner_cspace
+{
+namespace planner_3d
+{
+
+class StartPosePredictor
+{
+public:
+  using Astar = GridAstar<3, 2>;
+
+  struct Config
+  {
+    double prediction_sec_;
+    double switch_back_prediction_sec_;
+    double dist_stop_;
+    double lin_vel_;
+    double ang_vel_;
+  };
+  void setConfig(const Config& config)
+  {
+    config_ = config;
+    clear();
+  }
+  bool process(const geometry_msgs::Pose& robot_pose,
+               const GridAstar<3, 2>::Gridmap<char, 0x40>& cm,
+               const costmap_cspace_msgs::MapMetaData3D& map_info,
+               const nav_msgs::Path& previous_path_msg,
+               Astar::Vec& result_start_grid);
+  void clear();
+  const nav_msgs::Path& getPreservedPath() const
+  {
+    return preserved_path_;
+  }
+  const double getPreservedPathLength() const
+  {
+    return preserved_path_length_;
+  }
+
+private:
+  bool removeAlreadyPassed(const geometry_msgs::Pose& robot_pose);
+  double getInitialETA(
+      const geometry_msgs::Pose& robot_pose, const trajectory_tracker::Pose2D& initial_path_pose) const;
+  bool buildResults(const trajectory_tracker::Path2D::ConstIterator& expected_start_pose_it,
+                    const geometry_msgs::Pose& start_metric,
+                    const GridAstar<3, 2>::Gridmap<char, 0x40>& cm,
+                    Astar::Vec& start_grid);
+  bool isGridCenter(const trajectory_tracker::Pose2D& pose) const;
+  bool isPathColliding(const trajectory_tracker::Path2D::ConstIterator& begin,
+                       const trajectory_tracker::Path2D::ConstIterator& end,
+                       const GridAstar<3, 2>::Gridmap<char, 0x40>& cm);
+  trajectory_tracker::Path2D::ConstIterator getSwitchBack(const std::vector<double>& etas) const;
+  trajectory_tracker::Path2D::ConstIterator getExpectedPose(const std::vector<double>& etas) const;
+
+  Config config_;
+  costmap_cspace_msgs::MapMetaData3D map_info_;
+  trajectory_tracker::Path2D previous_path_2d_;
+  nav_msgs::Path preserved_path_;
+  double preserved_path_length_ = 0.0;
+};
+
+}  // namespace planner_3d
+}  // namespace planner_cspace
+
+#endif  // PLANNER_CSPACE_PLANNER_3D_START_POSE_PREDICTOR_H
diff --git a/planner_cspace/package.xml b/planner_cspace/package.xml
index c5a5ef84..5f401b1a 100644
--- a/planner_cspace/package.xml
+++ b/planner_cspace/package.xml
@@ -35,5 +35,6 @@
   <depend>neonavigation_common</depend>
   <depend>neonavigation_metrics_msgs</depend>
   <depend version_gte="0.12.0">planner_cspace_msgs</depend>
+  <depend>trajectory_tracker</depend>
   <depend>trajectory_tracker_msgs</depend>
 </package>
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
index f9ccb2c1..7f264b34 100644
--- a/planner_cspace/src/planner_3d.cpp
+++ b/planner_cspace/src/planner_3d.cpp
@@ -73,12 +73,13 @@
 #include <planner_cspace/grid_astar.h>
 #include <planner_cspace/jump_detector.h>
 #include <planner_cspace/planner_3d/costmap_bbf.h>
+#include <planner_cspace/planner_3d/distance_map.h>
 #include <planner_cspace/planner_3d/grid_astar_model.h>
 #include <planner_cspace/planner_3d/grid_metric_converter.h>
 #include <planner_cspace/planner_3d/motion_cache.h>
 #include <planner_cspace/planner_3d/path_interpolator.h>
 #include <planner_cspace/planner_3d/rotation_cache.h>
-#include <planner_cspace/planner_3d/distance_map.h>
+#include <planner_cspace/planner_3d/start_pose_predictor.h>
 
 namespace planner_cspace
 {
@@ -101,6 +102,7 @@ class Planner3dNode
   ros::Publisher pub_path_;
   ros::Publisher pub_path_velocity_;
   ros::Publisher pub_path_poses_;
+  ros::Publisher pub_preserved_path_poses_;
   ros::Publisher pub_hysteresis_map_;
   ros::Publisher pub_distance_map_;
   ros::Publisher pub_remembered_map_;
@@ -171,6 +173,7 @@ class Planner3dNode
   bool use_path_with_velocity_;
   bool retain_last_error_status_;
   int num_cost_estim_task_;
+  bool keep_a_part_of_previous_path_;
 
   JumpDetector jump_;
   std::string robot_frame_;
@@ -217,6 +220,8 @@ class Planner3dNode
   int prev_map_update_x_max_;
   int prev_map_update_y_min_;
   int prev_map_update_y_max_;
+  nav_msgs::Path previous_path_;
+  StartPosePredictor start_pose_predictor_;
 
   bool cbForget(std_srvs::EmptyRequest& req,
                 std_srvs::EmptyResponse& res)
@@ -360,7 +365,7 @@ class Planner3dNode
 
     const std::list<Astar::Vecf> path_interpolated =
         model_->path_interpolator_.interpolate(path_grid, 0.5, 0.0);
-    grid_metric_converter::grid2MetricPath(map_info_, path_interpolated, path);
+    grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path);
 
     res.plan.header = map_header_;
     res.plan.poses.resize(path.poses.size());
@@ -685,16 +690,7 @@ class Planner3dNode
     nav_msgs::Path path;
     path.header.frame_id = robot_frame_;
     path.header.stamp = ros::Time::now();
-    if (use_path_with_velocity_)
-    {
-      pub_path_velocity_.publish(
-          trajectory_tracker_msgs::toPathWithVelocity(
-              path, std::numeric_limits<double>::quiet_NaN()));
-    }
-    else
-    {
-      pub_path_.publish(path);
-    }
+    publishPath(path);
   }
   void publishFinishPath()
   {
@@ -708,7 +704,10 @@ class Planner3dNode
       path.poses[0].pose = goal_raw_.pose;
     else
       path.poses[0].pose = goal_.pose;
-
+    publishPath(path);
+  }
+  void publishPath(const nav_msgs::Path& path)
+  {
     if (use_path_with_velocity_)
     {
       pub_path_velocity_.publish(
@@ -719,6 +718,7 @@ class Planner3dNode
     {
       pub_path_.publish(path);
     }
+    previous_path_ = path;
   }
 
   void cbMapUpdate(const costmap_cspace_msgs::CSpace3DUpdate::ConstPtr msg)
@@ -1125,8 +1125,8 @@ class Planner3dNode
           nh_, "path",
           pnh_, "path", 1, true);
     }
-    pub_path_poses_ = pnh_.advertise<geometry_msgs::PoseArray>(
-        "path_poses", 1, true);
+    pub_path_poses_ = pnh_.advertise<geometry_msgs::PoseArray>("path_poses", 1, true);
+    pub_preserved_path_poses_ = pnh_.advertise<nav_msgs::Path>("preserved_path_poses", 1, true);
 
     pnh_.param("freq", freq_, 4.0f);
     pnh_.param("freq_min", freq_min_, 2.0f);
@@ -1345,6 +1345,24 @@ class Planner3dNode
           };
       cost_estim_cache_.init(model_, dmp);
     }
+
+    keep_a_part_of_previous_path_ = config.keep_a_part_of_previous_path;
+    StartPosePredictor::Config start_pose_predictor_config;
+    start_pose_predictor_config.lin_vel_ = config.max_vel;
+    start_pose_predictor_config.ang_vel_ = config.max_ang_vel;
+    start_pose_predictor_config.dist_stop_ = config.dist_stop_to_previous_path;
+    start_pose_predictor_config.prediction_sec_ = 1.0 / freq_;
+    start_pose_predictor_config.switch_back_prediction_sec_ = config.sw_wait;
+    if (keep_a_part_of_previous_path_)
+    {
+      // No need to wait additional times
+      sw_wait_ = 1.0 / freq_;
+    }
+    else
+    {
+      sw_wait_ = config.sw_wait;
+    }
+    start_pose_predictor_.setConfig(start_pose_predictor_config);
   }
 
   GridAstarModel3D::Vec pathPose2Grid(const geometry_msgs::PoseStamped& pose) const
@@ -1356,7 +1374,7 @@ class Planner3dNode
     return grid_vec;
   }
 
-  void waitUntil(const ros::Time& next_replan_time, const nav_msgs::Path& previous_path)
+  void waitUntil(const ros::Time& next_replan_time)
   {
     while (ros::ok())
     {
@@ -1375,9 +1393,9 @@ class Planner3dNode
           return;
         }
 
-        if (costmap_updated && previous_path.poses.size() > 1)
+        if (costmap_updated && previous_path_.poses.size() > 1)
         {
-          for (const auto& path_pose : previous_path.poses)
+          for (const auto& path_pose : previous_path_.poses)
           {
             if (cm_[pathPose2Grid(path_pose)] == 100)
             {
@@ -1416,11 +1434,10 @@ class Planner3dNode
     ROS_DEBUG("Initialized");
 
     ros::Time next_replan_time = ros::Time::now();
-    nav_msgs::Path previous_path;
 
     while (ros::ok())
     {
-      waitUntil(next_replan_time, previous_path);
+      waitUntil(next_replan_time);
 
       const ros::Time now = ros::Time::now();
       next_replan_time = now;
@@ -1444,7 +1461,6 @@ class Planner3dNode
                             last_costmap_.toSec());
           status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING;
           publishEmptyPath();
-          previous_path.poses.clear();
         }
         else
         {
@@ -1522,7 +1538,6 @@ class Planner3dNode
             has_goal_ = false;
 
             publishEmptyPath();
-            previous_path.poses.clear();
             next_replan_time += ros::Duration(1.0 / freq_);
             ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_);
 
@@ -1555,7 +1570,6 @@ class Planner3dNode
           if (skip_path_planning)
           {
             publishEmptyPath();
-            previous_path.poses.clear();
           }
           else
           {
@@ -1563,18 +1577,8 @@ class Planner3dNode
             path.header = map_header_;
             path.header.stamp = now;
             makePlan(start_.pose, goal_.pose, path, true);
-            if (use_path_with_velocity_)
-            {
-              // NaN velocity means that don't care the velocity
-              pub_path_velocity_.publish(
-                  trajectory_tracker_msgs::toPathWithVelocity(path, std::numeric_limits<double>::quiet_NaN()));
-            }
-            else
-            {
-              pub_path_.publish(path);
-            }
-            previous_path = path;
-            if (sw_wait_ > 0.0)
+            publishPath(path);
+            if ((sw_wait_ > 0.0) && !keep_a_part_of_previous_path_)
             {
               const int sw_index = getSwitchIndex(path);
               is_path_switchback = (sw_index >= 0);
@@ -1589,7 +1593,6 @@ class Planner3dNode
         if (!retain_last_error_status_)
           status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
         publishEmptyPath();
-        previous_path.poses.clear();
       }
       status_.header.stamp = now;
       pub_status_.publish(status_);
@@ -1642,6 +1645,24 @@ class Planner3dNode
     pub_start_.publish(p);
   }
 
+  bool isPathFinishing(const Astar::Vec& start_grid, const Astar::Vec& end_grid) const
+  {
+    int g_tolerance_lin, g_tolerance_ang;
+    if (act_tolerant_->isActive())
+    {
+      g_tolerance_lin = std::lround(goal_tolerant_->goal_tolerance_lin / map_info_.linear_resolution);
+      g_tolerance_ang = std::lround(goal_tolerant_->goal_tolerance_ang / map_info_.angular_resolution);
+    }
+    else
+    {
+      g_tolerance_lin = goal_tolerance_lin_;
+      g_tolerance_ang = goal_tolerance_ang_;
+    }
+    Astar::Vec remain = start_grid - end_grid;
+    remain.cycle(map_info_.angle);
+    return (remain.sqlen() <= g_tolerance_lin * g_tolerance_lin && std::abs(remain[2]) <= g_tolerance_ang);
+  }
+
   enum class StartPoseStatus
   {
     START_OCCUPIED,
@@ -1649,8 +1670,11 @@ class Planner3dNode
     NORMAL,
   };
   StartPoseStatus buildStartPoses(const geometry_msgs::Pose& start_metric, const geometry_msgs::Pose& end_metric,
-                                  std::vector<Astar::VecWithCost>& result_start_poses) const
+                                  std::vector<Astar::VecWithCost>& result_start_poses)
   {
+    result_start_poses.clear();
+    start_pose_predictor_.clear();
+
     Astar::Vecf sf;
     grid_metric_converter::metric2Grid(
         map_info_, sf[0], sf[1], sf[2],
@@ -1669,6 +1693,24 @@ class Planner3dNode
       ROS_ERROR("You are on the edge of the world.");
       return StartPoseStatus::START_OCCUPIED;
     }
+
+    if (keep_a_part_of_previous_path_ && !previous_path_.poses.empty())
+    {
+      Astar::Vec expected_start_grid;
+      if (start_pose_predictor_.process(start_metric, cm_, map_info_, previous_path_, expected_start_grid))
+      {
+        ROS_DEBUG("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by start pose predictor.",
+                  expected_start_grid[0], expected_start_grid[1], expected_start_grid[2],
+                  start_grid[0], start_grid[1], start_grid[2]);
+        result_start_poses.push_back(Astar::VecWithCost(expected_start_grid));
+        return isPathFinishing(start_grid, end_grid) ? StartPoseStatus::FINISHING : StartPoseStatus::NORMAL;
+      }
+      else
+      {
+        return StartPoseStatus::START_OCCUPIED;
+      }
+    }
+
     if (antialias_start_)
     {
       const int x_cand[] = {0, ((sf[0] - start_grid[0]) < 0.5 ? -1 : 1)};
@@ -1700,32 +1742,20 @@ class Planner3dNode
     }
     if (result_start_poses.empty())
     {
+      const Astar::Vec original_start_grid = start_grid;
       if (!searchAvailablePos(cm_, start_grid, tolerance_range_, tolerance_angle_))
       {
         ROS_WARN("Oops! You are in Rock!");
         return StartPoseStatus::START_OCCUPIED;
       }
-      ROS_INFO("Start moved");
+      ROS_INFO("Start grid is moved to (%d, %d, %d) from (%d, %d, %d) by relocation.",
+               start_grid[0], start_grid[1], start_grid[2],
+               original_start_grid[0], original_start_grid[1], original_start_grid[2]);
       result_start_poses.push_back(Astar::VecWithCost(start_grid));
     }
-    int g_tolerance_lin, g_tolerance_ang;
-    if (act_tolerant_->isActive())
-    {
-      g_tolerance_lin = std::lround(goal_tolerant_->goal_tolerance_lin / map_info_.linear_resolution);
-      g_tolerance_ang = std::lround(goal_tolerant_->goal_tolerance_ang / map_info_.angular_resolution);
-    }
-    else
-    {
-      g_tolerance_lin = goal_tolerance_lin_;
-      g_tolerance_ang = goal_tolerance_ang_;
-    }
     for (const Astar::VecWithCost& s : result_start_poses)
     {
-      // Check if arrived to the goal
-      Astar::Vec remain = s.v_ - end_grid;
-      remain.cycle(map_info_.angle);
-      if (remain.sqlen() <= g_tolerance_lin * g_tolerance_lin &&
-          std::abs(remain[2]) <= g_tolerance_ang)
+      if (isPathFinishing(s.v_, end_grid))
       {
         return StartPoseStatus::FINISHING;
       }
@@ -1788,6 +1818,7 @@ class Planner3dNode
     if (initial_2dof_cost == std::numeric_limits<float>::max() || cm_[end_grid] >= 100)
     {
       status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
+      start_pose_predictor_.clear();
       ROS_WARN("Goal unreachable.");
       if (!escaping_ && temporary_escape_)
       {
@@ -1838,12 +1869,23 @@ class Planner3dNode
 
     model_->enableHysteresis(hyst && has_hysteresis_map_);
     std::list<Astar::Vec> path_grid;
-    if (!as_.search(starts, end_grid, path_grid,
-                    model_,
-                    cb_progress,
-                    range_limit,
-                    1.0f / freq_min_,
-                    find_best_))
+    bool is_goal_same_as_start = false;
+    for (const auto s : starts)
+    {
+      if (s.v_ == end_grid)
+      {
+        ROS_DEBUG("The start grid is the same as the end grid. Path planning skipped.");
+        path_grid.push_back(end_grid);
+        is_goal_same_as_start = true;
+        break;
+      }
+    }
+    if (!is_goal_same_as_start && !as_.search(starts, end_grid, path_grid,
+                                              model_,
+                                              cb_progress,
+                                              range_limit,
+                                              1.0f / freq_min_,
+                                              find_best_))
     {
       ROS_WARN("Path plan failed (goal unreachable)");
       status_.error = planner_cspace_msgs::PlannerStatus::PATH_NOT_FOUND;
@@ -1872,10 +1914,14 @@ class Planner3dNode
       poses.poses.push_back(pose);
     }
     pub_path_poses_.publish(poses);
-
+    if (!start_pose_predictor_.getPreservedPath().poses.empty())
+    {
+      pub_preserved_path_poses_.publish(start_pose_predictor_.getPreservedPath());
+    }
     const std::list<Astar::Vecf> path_interpolated =
         model_->path_interpolator_.interpolate(path_grid, 0.5, local_range_);
-    grid_metric_converter::grid2MetricPath(map_info_, path_interpolated, path);
+    path.poses = start_pose_predictor_.getPreservedPath().poses;
+    grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path);
 
     if (hyst)
     {
diff --git a/planner_cspace/src/start_pose_predictor.cpp b/planner_cspace/src/start_pose_predictor.cpp
new file mode 100644
index 00000000..759c541f
--- /dev/null
+++ b/planner_cspace/src/start_pose_predictor.cpp
@@ -0,0 +1,247 @@
+/*
+ * Copyright (c) 2023, the neonavigation authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <algorithm>
+#include <vector>
+
+#include <ros/ros.h>
+#include <tf2/utils.h>
+
+#include <planner_cspace/planner_3d/start_pose_predictor.h>
+
+namespace planner_cspace
+{
+namespace planner_3d
+{
+bool StartPosePredictor::process(const geometry_msgs::Pose& robot_pose,
+                                 const GridAstar<3, 2>::Gridmap<char, 0x40>& cm,
+                                 const costmap_cspace_msgs::MapMetaData3D& map_info,
+                                 const nav_msgs::Path& previous_path_msg,
+                                 StartPosePredictor::Astar::Vec& result_start_grid)
+{
+  clear();
+  preserved_path_.header = previous_path_msg.header;
+  previous_path_2d_.fromMsg(previous_path_msg);
+  map_info_ = map_info;
+
+  if (previous_path_2d_.empty() || !removeAlreadyPassed(robot_pose))
+  {
+    clear();
+    return false;
+  }
+  const double initial_eta = getInitialETA(robot_pose, previous_path_2d_.front());
+  const std::vector<double> etas = previous_path_2d_.getEstimatedTimeOfArrivals(
+      previous_path_2d_.begin(), previous_path_2d_.end(), config_.lin_vel_, config_.ang_vel_, initial_eta);
+
+  const trajectory_tracker::Path2D::ConstIterator local_goal_it = getSwitchBack(etas);
+  if (local_goal_it != previous_path_2d_.end())
+  {
+    return buildResults(local_goal_it, robot_pose, cm, result_start_grid);
+  }
+  const trajectory_tracker::Path2D::ConstIterator expected_pose_it = getExpectedPose(etas);
+  if (expected_pose_it != previous_path_2d_.end())
+  {
+    return buildResults(expected_pose_it, robot_pose, cm, result_start_grid);
+  }
+  ROS_DEBUG("The robot will reach the goal in %f sec.", etas.back());
+  return buildResults(previous_path_2d_.end() - 1, robot_pose, cm, result_start_grid);
+}
+
+void planner_cspace::planner_3d::StartPosePredictor::clear()
+{
+  previous_path_2d_.clear();
+  preserved_path_.poses.clear();
+  preserved_path_length_ = 0.0;
+}
+
+bool StartPosePredictor::removeAlreadyPassed(const geometry_msgs::Pose& start_metric)
+{
+  const Eigen::Vector2d robot_pose_2d(start_metric.position.x, start_metric.position.y);
+  double dist_err;
+  trajectory_tracker::Path2D::ConstIterator it_nearest;
+  std::tie(it_nearest, dist_err) =
+      previous_path_2d_.findNearestWithDistance(previous_path_2d_.begin(), previous_path_2d_.end(), robot_pose_2d);
+  if (dist_err > config_.dist_stop_)
+  {
+    ROS_WARN("The robot is too far from path. An empty path is published. dist: %f, thr: %f",
+             dist_err, config_.dist_stop_);
+    return false;
+  }
+  if (previous_path_2d_.size() == 1)
+  {
+    return true;
+  }
+  size_t nearest_pos = it_nearest - previous_path_2d_.begin();
+  if (nearest_pos == 1)
+  {
+    // findNearestWithDistance() never returns begin() and (nearest_pos == 1) means that the first line strip is the
+    // nearest. In this case, no points should be removed when the robot is behind the path.
+    const double distance_from_start = std::hypot(start_metric.position.x - previous_path_2d_.begin()->pos_.x(),
+                                                  start_metric.position.y - previous_path_2d_.begin()->pos_.y());
+    if (std::abs(distance_from_start - dist_err) < 1.0e-6)
+    {
+      ROS_DEBUG("Nearest pose in previous path: (%f, %f, %f), dist: %f, index: 0, remaining poses num: %lu",
+                previous_path_2d_.begin()->pos_.x(), previous_path_2d_.begin()->pos_.y(),
+                previous_path_2d_.begin()->yaw_, dist_err, previous_path_2d_.size());
+      return true;
+    }
+  }
+  ROS_DEBUG("Nearest pose in previous path: (%f, %f, %f), dist: %f, index: %lu, remaining poses num: %lu",
+            it_nearest->pos_.x(), it_nearest->pos_.y(), it_nearest->yaw_, dist_err, nearest_pos,
+            previous_path_2d_.size() - nearest_pos);
+  previous_path_2d_.erase(previous_path_2d_.begin(), it_nearest);
+  return true;
+}
+
+double StartPosePredictor::getInitialETA(
+    const geometry_msgs::Pose& robot_pose, const trajectory_tracker::Pose2D& initial_path_pose) const
+{
+  double angle_diff = std::abs(initial_path_pose.yaw_ - tf2::getYaw(robot_pose.orientation));
+  angle_diff = (angle_diff > M_PI) ? 2 * M_PI - angle_diff : angle_diff;  // Normalize
+  const double dist_diff = std::hypot(initial_path_pose.pos_.x() - robot_pose.position.x,
+                                      initial_path_pose.pos_.y() - robot_pose.position.y);
+  return std::max(std::abs(angle_diff) / config_.ang_vel_, dist_diff / config_.lin_vel_);
+}
+
+bool StartPosePredictor::buildResults(
+    const trajectory_tracker::Path2D::ConstIterator& expected_start_pose_it,
+    const geometry_msgs::Pose& robot_pose,
+    const GridAstar<3, 2>::Gridmap<char, 0x40>& cm,
+    StartPosePredictor::Astar::Vec& start_grid)
+{
+  if (isPathColliding(previous_path_2d_.begin(), expected_start_pose_it, cm))
+  {
+    ROS_WARN("The robot might collide with an obstacle during the next planning. An empty path is published.");
+    clear();
+    return false;
+  }
+
+  grid_metric_converter::metric2Grid(
+      map_info_, start_grid[0], start_grid[1], start_grid[2],
+      expected_start_pose_it->pos_.x(), expected_start_pose_it->pos_.y(), expected_start_pose_it->yaw_);
+  start_grid.cycleUnsigned(map_info_.angle);
+  const size_t previous_path_size = previous_path_2d_.size();
+  previous_path_2d_.erase(expected_start_pose_it + 1, previous_path_2d_.end());
+  ROS_DEBUG("Robot Pose: (%f, %f, %f), Start Pose: (%f, %f, %f), Start Grid: (%d, %d, %d), path size: %lu -> %lu",
+            robot_pose.position.x, robot_pose.position.y, tf2::getYaw(robot_pose.orientation),
+            expected_start_pose_it->pos_.x(), expected_start_pose_it->pos_.y(), expected_start_pose_it->yaw_,
+            start_grid[0], start_grid[1], start_grid[2], previous_path_size, previous_path_2d_.size());
+  if (!previous_path_2d_.empty())
+  {
+    preserved_path_length_ = previous_path_2d_.length();
+    previous_path_2d_.toMsg(preserved_path_);
+    // The last pose will be the first pose of the next planning, and it should be removed from preserved_path_.
+    preserved_path_.poses.pop_back();
+  }
+  return true;
+}
+
+bool StartPosePredictor::isGridCenter(const trajectory_tracker::Pose2D& pose) const
+{
+  // Return true when all of the x, y and yaw in the grid coordinate are integers.
+  Astar::Vecf grid;
+  grid_metric_converter::metric2Grid(map_info_, grid[0], grid[1], grid[2], pose.pos_.x(), pose.pos_.y(), pose.yaw_);
+  const float x_diff = std::abs(grid[0] - 0.5 - std::round(grid[0] - 0.5));
+  const float y_diff = std::abs(grid[1] - 0.5 - std::round(grid[1] - 0.5));
+  const float r_diff = std::abs(grid[2] - std::round(grid[2]));
+  return (x_diff < 1.0e-3) && (y_diff < 1.0e-3) && (r_diff < 1.0e-3);
+}
+
+bool StartPosePredictor::isPathColliding(const trajectory_tracker::Path2D::ConstIterator& begin,
+                                         const trajectory_tracker::Path2D::ConstIterator& end,
+                                         const GridAstar<3, 2>::Gridmap<char, 0x40>& cm)
+{
+  for (auto it = begin; it != end; ++it)
+  {
+    Astar::Vec grid;
+    grid_metric_converter::metric2Grid(map_info_, grid[0], grid[1], grid[2], it->pos_.x(), it->pos_.y(), it->yaw_);
+    grid.cycleUnsigned(map_info_.angle);
+    if (cm[grid] == 100)
+    {
+      ROS_DEBUG("Path colliding at (%f, %f, %f)", it->pos_.x(), it->pos_.y(), it->yaw_);
+      return true;
+    }
+  }
+  return false;
+}
+
+trajectory_tracker::Path2D::ConstIterator StartPosePredictor::getSwitchBack(const std::vector<double>& etas) const
+{
+  const auto local_goal_its = previous_path_2d_.enumerateLocalGoals(
+      previous_path_2d_.begin(), previous_path_2d_.end(), true, false);
+  if (local_goal_its.empty())
+  {
+    return previous_path_2d_.end();
+  }
+
+  for (const auto& local_goal : local_goal_its)
+  {
+    // Note that Path2D::findNearest() never returns the begin iterator and we can safely subtract 1.
+    const size_t local_goal_index = local_goal - previous_path_2d_.begin() - 1;
+    const double local_goal_eta = etas[local_goal_index];
+    ROS_DEBUG("Switch back index: %lu, eta: %f", local_goal_index, local_goal_eta);
+    if (isGridCenter(previous_path_2d_[local_goal_index]))
+    {
+      if ((config_.prediction_sec_ < local_goal_eta) &&
+          (local_goal_eta < config_.switch_back_prediction_sec_))
+      {
+        ROS_INFO("The robot will reach a switch back point in %f sec. The next plan starts from the switch back.",
+                 local_goal_eta);
+        return local_goal - 1;
+      }
+    }
+    else
+    {
+      ROS_WARN("The switch back point is not placed on a grid center. (%f, %f, %f) -> (%f, %f, %f)",
+               previous_path_2d_[local_goal_index].pos_.x(), previous_path_2d_[local_goal_index].pos_.y(),
+               previous_path_2d_[local_goal_index].yaw_, previous_path_2d_[local_goal_index + 1].pos_.x(),
+               previous_path_2d_[local_goal_index + 1].pos_.y(), previous_path_2d_[local_goal_index + 1].yaw_);
+    }
+  }
+  return previous_path_2d_.end();
+}
+
+trajectory_tracker::Path2D::ConstIterator StartPosePredictor::getExpectedPose(const std::vector<double>& etas) const
+{
+  for (size_t i = 0; i < previous_path_2d_.size() - 1; ++i)
+  {
+    if (etas[i] > config_.prediction_sec_)
+    {
+      if (isGridCenter(previous_path_2d_[i]))
+      {
+        ROS_DEBUG("The robot will reach a grid center in %f sec.", etas[i]);
+        return previous_path_2d_.begin() + i;
+      }
+    }
+  }
+  return previous_path_2d_.end();
+}
+
+}  // namespace planner_3d
+}  // namespace planner_cspace
diff --git a/planner_cspace/test/CMakeLists.txt b/planner_cspace/test/CMakeLists.txt
index af80c231..e78c749e 100644
--- a/planner_cspace/test/CMakeLists.txt
+++ b/planner_cspace/test/CMakeLists.txt
@@ -67,6 +67,14 @@ catkin_add_gtest(test_path_interpolator
 )
 target_link_libraries(test_path_interpolator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
+catkin_add_gtest(test_start_pose_predictor
+  src/test_start_pose_predictor.cpp
+  ../src/path_interpolator.cpp
+  ../src/rotation_cache.cpp
+  ../src/start_pose_predictor.cpp
+)
+target_link_libraries(test_start_pose_predictor ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
 add_rostest_gtest(test_debug_outputs
   test/debug_outputs_rostest.test
   src/test_debug_outputs.cpp
diff --git a/planner_cspace/test/src/test_dynamic_parameter_change.cpp b/planner_cspace/test/src/test_dynamic_parameter_change.cpp
index a822431b..aed50e65 100644
--- a/planner_cspace/test/src/test_dynamic_parameter_change.cpp
+++ b/planner_cspace/test/src/test_dynamic_parameter_change.cpp
@@ -34,8 +34,12 @@
 
 #include <dynamic_reconfigure/client.h>
 #include <move_base_msgs/MoveBaseAction.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
 #include <planner_cspace/Planner3DConfig.h>
 #include <ros/ros.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
 
 #include <planner_cspace/action_test_base.h>
 
@@ -45,11 +49,38 @@ class DynamicParameterChangeTest
 public:
   void SetUp() final
   {
-    ActionTestBase<move_base_msgs::MoveBaseAction, ACTION_TOPIC_MOVE_BASE>::SetUp();
     path_ = nullptr;
     planner_3d_client_.reset(
         new dynamic_reconfigure::Client<planner_cspace::Planner3DConfig>("/planner_3d/"));
     sub_path_ = node_.subscribe("path", 1, &DynamicParameterChangeTest::cbPath, this);
+    pub_map_overlay_ = node_.advertise<nav_msgs::OccupancyGrid>("map_overlay", 1, true);
+    pub_odom_ = node_.advertise<nav_msgs::Odometry>("odom", 1, true);
+
+    map_overlay_.header.frame_id = "map";
+    map_overlay_.info.resolution = 0.1;
+    map_overlay_.info.width = 32;
+    map_overlay_.info.height = 32;
+    map_overlay_.info.origin.position.x = 0.0;
+    map_overlay_.info.origin.position.y = 0.0;
+    map_overlay_.info.origin.position.z = 0.0;
+    map_overlay_.info.origin.orientation.x = 0.0;
+    map_overlay_.info.origin.orientation.y = 0.0;
+    map_overlay_.info.origin.orientation.z = 0.0;
+    map_overlay_.info.origin.orientation.w = 1.0;
+    map_overlay_.data.resize(map_overlay_.info.width * map_overlay_.info.height, 0);
+    publishMapAndRobot(0, 0, 0);
+    ActionTestBase<move_base_msgs::MoveBaseAction, ACTION_TOPIC_MOVE_BASE>::SetUp();
+
+    default_config_ = planner_cspace::Planner3DConfig::__getDefault__();
+    default_config_.max_retry_num = 5;
+    default_config_.tolerance_range = 0.0;
+    default_config_.temporary_escape = false;
+    default_config_.goal_tolerance_lin = 0.05;
+    default_config_.cost_in_place_turn = 3.0;
+    default_config_.min_curve_radius = 0.4;
+    default_config_.max_vel = 0.3;
+    default_config_.max_ang_vel = 1.0;
+    ASSERT_TRUE(planner_3d_client_->setConfiguration(default_config_));
   }
 
 protected:
@@ -90,50 +121,153 @@ class DynamicParameterChangeTest
     return false;
   }
 
+  ::testing::AssertionResult comparePath(const nav_msgs::Path& path1, const nav_msgs::Path& path2)
+  {
+    if (path1.poses.size() != path2.poses.size())
+    {
+      return ::testing::AssertionFailure()
+             << "Path size different: " << path1.poses.size() << " != " << path2.poses.size();
+    }
+    for (size_t i = 0; i < path1.poses.size(); ++i)
+    {
+      const geometry_msgs::Point& pos1 = path1.poses[i].pose.position;
+      const geometry_msgs::Point& pos2 = path2.poses[i].pose.position;
+      if (std::abs(pos1.x - pos2.x) > 1.0e-6)
+      {
+        return ::testing::AssertionFailure() << "X different at #" << i << ": " << pos1.x << " != " << pos2.x;
+      }
+      if (std::abs(pos1.y - pos2.y) > 1.0e-6)
+      {
+        return ::testing::AssertionFailure() << "Y different at #" << i << ": " << pos1.y << " != " << pos2.y;
+      }
+      const double yaw1 = tf2::getYaw(path1.poses[i].pose.orientation);
+      const double yaw2 = tf2::getYaw(path2.poses[i].pose.orientation);
+      if (std::abs(yaw1 - yaw2) > 1.0e-6)
+      {
+        return ::testing::AssertionFailure() << "Yaw different at #" << i << ": " << yaw1 << " != " << yaw2;
+      }
+    }
+    return ::testing::AssertionSuccess();
+  }
+
+  void sendGoalAndWaitForPath()
+  {
+    path_ = nullptr;
+    const ros::Time start_time = ros::Time::now();
+    ros::Time deadline = start_time + ros::Duration(1.0);
+    move_base_->sendGoal(CreateGoalInFree());
+    while (ros::ok())
+    {
+      if (path_ && (path_->header.stamp > start_time) && (path_->poses.size() > 0))
+      {
+        break;
+      }
+      ASSERT_LT(ros::Time::now(), deadline)
+          << "Faile to plan:" << move_base_->getState().toString() << statusString();
+      ros::spinOnce();
+    }
+  }
+
+  void publishMapAndRobot(const double x, const double y, const double yaw)
+  {
+    const ros::Time current_time = ros::Time::now();
+    geometry_msgs::TransformStamped trans;
+    trans.header.stamp = current_time;
+    trans.header.frame_id = "odom";
+    trans.child_frame_id = "base_link";
+    trans.transform.translation = tf2::toMsg(tf2::Vector3(x, y, 0.0));
+    trans.transform.rotation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
+    tfb_.sendTransform(trans);
+
+    nav_msgs::Odometry odom;
+    odom.header.frame_id = "odom";
+    odom.header.stamp = current_time;
+    odom.child_frame_id = "base_link";
+    odom.pose.pose.position.x = x;
+    odom.pose.pose.position.y = y;
+    odom.pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), yaw));
+    pub_odom_.publish(odom);
+
+    pub_map_overlay_.publish(map_overlay_);
+  }
+
+  tf2_ros::TransformBroadcaster tfb_;
   ros::Subscriber sub_path_;
   nav_msgs::Path::ConstPtr path_;
   std::unique_ptr<dynamic_reconfigure::Client<planner_cspace::Planner3DConfig>> planner_3d_client_;
+  ros::Publisher pub_map_overlay_;
+  ros::Publisher pub_odom_;
+  nav_msgs::OccupancyGrid map_overlay_;
+  planner_cspace::Planner3DConfig default_config_;
 };
 
 TEST_F(DynamicParameterChangeTest, DisableCurves)
 {
-  ros::Time deadline = ros::Time::now() + ros::Duration(1.0);
-  move_base_->sendGoal(CreateGoalInFree());
-  while (ros::ok())
-  {
-    if (path_ && (path_->poses.size() > 0))
-    {
-      break;
-    }
-    ASSERT_LT(ros::Time::now(), deadline)
-        << "Faile to plan:" << move_base_->getState().toString() << statusString();
-    ros::spinOnce();
-  }
+  publishMapAndRobot(2.55, 0.45, M_PI);
+  ros::Duration(0.5).sleep();
+
+  sendGoalAndWaitForPath();
   // The default path is including curves.
   EXPECT_TRUE(isPathIncludingCurves());
 
-  planner_cspace::Planner3DConfig config;
-  ASSERT_TRUE(planner_3d_client_->getCurrentConfiguration(config, ros::Duration(0.1)));
+  planner_cspace::Planner3DConfig config = default_config_;
   // Large min_curve_radius disables curves.
   config.min_curve_radius = 10.0;
   ASSERT_TRUE(planner_3d_client_->setConfiguration(config));
 
-  const ros::Time config_sent_time = ros::Time::now();
-  deadline = config_sent_time + ros::Duration(1.0);
-  path_ = nullptr;
-  while (ros::ok())
-  {
-    if (path_ && (path_->poses.size() > 0) && (path_->header.stamp > config_sent_time))
-    {
-      break;
-    }
-    ASSERT_LT(ros::Time::now(), deadline)
-        << "Faile to plan:" << move_base_->getState().toString() << statusString();
-    ros::spinOnce();
-  }
+  sendGoalAndWaitForPath();
+  // The default path is including curves.
   EXPECT_FALSE(isPathIncludingCurves());
 }
 
+TEST_F(DynamicParameterChangeTest, StartPosePrediction)
+{
+  publishMapAndRobot(1.65, 0.65, M_PI);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  const nav_msgs::Path initial_path = *path_;
+
+  // The path is changed to keep distance from the obstacle.
+  map_overlay_.data[13 + 5 * map_overlay_.info.width] = 100;
+  publishMapAndRobot(1.65, 0.65, M_PI);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  EXPECT_FALSE(comparePath(initial_path, *path_));
+
+  // Enable start pose prediction.
+  move_base_->cancelAllGoals();
+  planner_cspace::Planner3DConfig config = default_config_;
+  config.keep_a_part_of_previous_path = true;
+  config.dist_stop_to_previous_path = 0.1;
+  ASSERT_TRUE(planner_3d_client_->setConfiguration(config));
+
+  // No obstacle and the path is same as the first one.
+  map_overlay_.data[13 + 5 * map_overlay_.info.width] = 0;
+  publishMapAndRobot(1.65, 0.65, M_PI);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  EXPECT_TRUE(comparePath(initial_path, *path_));
+
+  // The path does not change as a part of the previous path is kept and it is not possible to keep distance from
+  // the obstacle.
+  map_overlay_.data[13 + 5 * map_overlay_.info.width] = 100;
+  publishMapAndRobot(1.65, 0.65, M_PI);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  EXPECT_TRUE(comparePath(initial_path, *path_));
+
+  // It is expected that the robot reaches the goal during the path planning.
+  move_base_->cancelAllGoals();
+  map_overlay_.data[13 + 5 * map_overlay_.info.width] = 0;
+  publishMapAndRobot(1.25, 0.95, M_PI / 2);
+  ros::Duration(0.5).sleep();
+  sendGoalAndWaitForPath();
+  const nav_msgs::Path short_path = *path_;
+  // In the second path planning after cancel, the exptected start pose is same as the goal.
+  sendGoalAndWaitForPath();
+  EXPECT_TRUE(comparePath(short_path, *path_));
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/planner_cspace/test/src/test_grid_metric_converter.cpp b/planner_cspace/test/src/test_grid_metric_converter.cpp
index 509972a7..b0a3c7cb 100644
--- a/planner_cspace/test/src/test_grid_metric_converter.cpp
+++ b/planner_cspace/test/src/test_grid_metric_converter.cpp
@@ -119,7 +119,7 @@ TEST(GridMetricConverter, Path)
     path_grid.push_back(CyclicVecFloat<3, 2>(g[0], g[1], g[2]));
 
   nav_msgs::Path path;
-  grid_metric_converter::grid2MetricPath(map_info, path_grid, path);
+  grid_metric_converter::appendGridPath2MetricPath(map_info, path_grid, path);
   size_t ref = 0;
   for (const geometry_msgs::PoseStamped& p : path.poses)
   {
diff --git a/planner_cspace/test/src/test_start_pose_predictor.cpp b/planner_cspace/test/src/test_start_pose_predictor.cpp
new file mode 100644
index 00000000..7ed3f191
--- /dev/null
+++ b/planner_cspace/test/src/test_start_pose_predictor.cpp
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2023, the neonavigation authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+
+#include <Eigen/Core>
+#include <list>
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+#include <nav_msgs/Path.h>
+#include <planner_cspace/planner_3d/path_interpolator.h>
+#include <planner_cspace/planner_3d/start_pose_predictor.h>
+
+#include <ros/ros.h>
+
+namespace planner_cspace
+{
+namespace planner_3d
+{
+class StartPosePredictorTester : public ::testing::Test
+{
+protected:
+  void SetUp() final
+  {
+    config_ =
+        {
+            .prediction_sec_ = 0.5,
+            .switch_back_prediction_sec_ = 2.0,
+            .dist_stop_ = 0.2,
+            .lin_vel_ = 1.5,
+            .ang_vel_ = M_PI / 2,
+        };
+    predictor_.setConfig(config_);
+
+    map_info_.width = 11;
+    map_info_.height = 11;
+    map_info_.angle = 4;
+    map_info_.origin.position.x = -5.5;
+    map_info_.origin.position.y = -5.5;
+    map_info_.origin.orientation.w = 1.0;
+    map_info_.linear_resolution = 1.0;
+    map_info_.angular_resolution = 2 * M_PI / map_info_.angle;
+    interpolator_.reset(map_info_.linear_resolution, 1);
+
+    const GridAstar<3, 2>::Vec size3d(
+        static_cast<int>(map_info_.width),
+        static_cast<int>(map_info_.height),
+        static_cast<int>(map_info_.angle));
+    cm_.reset(size3d);
+    for (int a = 0; a < static_cast<int>(map_info_.angle); ++a)
+    {
+      for (int y = 0; y < static_cast<int>(map_info_.height); ++y)
+      {
+        for (int x = 0; x < static_cast<int>(map_info_.width); ++x)
+        {
+          cm_[GridAstar<3, 2>::Vec(x, y, a)] = 0;
+        }
+      }
+    }
+
+    const std::list<StartPosePredictor::Astar::Vec> path_grid_straight =
+        {
+            StartPosePredictor::Astar::Vec(5, 5, 0),  // Start is (0.0, 0.0, 0.0)
+            StartPosePredictor::Astar::Vec(6, 5, 0),
+            StartPosePredictor::Astar::Vec(7, 5, 0),  // Moves 2 meters ahead
+        };
+    paths_["straight"] = std::make_pair(path_grid_straight, convertToMetricPath(path_grid_straight));
+
+    const std::list<StartPosePredictor::Astar::Vec> path_grid_switch_back =
+        {
+            StartPosePredictor::Astar::Vec(5, 5, 0),  // Start is (0.0, 0.0, 0.0)
+            StartPosePredictor::Astar::Vec(6, 5, 0),
+            StartPosePredictor::Astar::Vec(7, 5, 0),
+            StartPosePredictor::Astar::Vec(8, 6, 1),   // Curv from (2.0, 0.0, 0.0) to (3.0, 1.0, pi/2)
+            StartPosePredictor::Astar::Vec(9, 5, 2),   // Curv from (3.0, 1.0, 0.0) to (4.0, 0.0, pi)
+            StartPosePredictor::Astar::Vec(10, 5, 2),  // goal is (5.0, 0.0, pi)
+        };
+    paths_["switch_back"] = std::make_pair(path_grid_switch_back, convertToMetricPath(path_grid_switch_back));
+  }
+
+  nav_msgs::Path convertToMetricPath(const std::list<GridAstar<3, 2>::Vec>& path_grid) const
+  {
+    const auto path_interpolated = interpolator_.interpolate(path_grid, 0.1, 10);
+
+    nav_msgs::Path result_path;
+    result_path.header.frame_id = "map";
+    for (const auto& pose_grid : path_interpolated)
+    {
+      geometry_msgs::PoseStamped pose;
+      pose.header.frame_id = "map";
+      float x, y, yaw;
+      grid_metric_converter::grid2Metric(map_info_, pose_grid[0], pose_grid[1], pose_grid[2], x, y, yaw);
+      pose.pose = getPose(x, y, yaw);
+      result_path.poses.push_back(pose);
+    }
+    return result_path;
+  }
+
+  geometry_msgs::Pose getPose(const double x, const double y, const double yaw) const
+  {
+    geometry_msgs::Pose pose;
+    pose.position.x = x;
+    pose.position.y = y;
+    pose.orientation.z = std::sin(yaw / 2);
+    pose.orientation.w = std::cos(yaw / 2);
+    return pose;
+  }
+
+  StartPosePredictor predictor_;
+  StartPosePredictor::Config config_;
+  GridAstar<3, 2>::Gridmap<char, 0x40> cm_;
+  costmap_cspace_msgs::MapMetaData3D map_info_;
+  PathInterpolator interpolator_;
+  std::unordered_map<std::string, std::pair<std::list<StartPosePredictor::Astar::Vec>, nav_msgs::Path>> paths_;
+};
+
+TEST_F(StartPosePredictorTester, EdgeCases)
+{
+  StartPosePredictor::Astar::Vec start_grid;
+  // Empty path
+  EXPECT_FALSE(predictor_.process(getPose(0.0, 0.0, 0.0), cm_, map_info_, nav_msgs::Path(), start_grid));
+
+  // Far from path
+  EXPECT_FALSE(predictor_.process(getPose(0.0, 0.5, 0.0), cm_, map_info_,
+                                  paths_["straight"].second, start_grid));
+
+  // Only the last pose is remaining
+  nav_msgs::Path rotating_path;
+  rotating_path.header.frame_id = "map";
+  rotating_path.poses.resize(1);
+  rotating_path.poses[0].header.frame_id = "map";
+  rotating_path.poses[0].pose = getPose(0.01, 1.01, M_PI / 2);
+
+  EXPECT_TRUE(predictor_.process(getPose(0.0, 1.0, 1.3), cm_, map_info_, rotating_path, start_grid));
+  EXPECT_EQ(StartPosePredictor::Astar::Vec(5, 6, 1), start_grid);
+  EXPECT_TRUE(predictor_.getPreservedPath().poses.empty());
+}
+
+TEST_F(StartPosePredictorTester, Normal)
+{
+  {
+    StartPosePredictor::Astar::Vec start_grid;
+    EXPECT_TRUE(predictor_.process(getPose(0.05, 0.05, 0.0), cm_, map_info_, paths_["straight"].second, start_grid));
+    EXPECT_EQ(StartPosePredictor::Astar::Vec(6, 5, 0), start_grid);
+    const auto preserved_path = predictor_.getPreservedPath();
+    // Only (0, 0, 0) is removed as the nearest is the first line strip, that is from (0, 0, 0) to (0.1, 0, 0).
+    ASSERT_EQ(preserved_path.poses.size(), 9);
+    for (size_t i = 0; i < preserved_path.poses.size(); ++i)
+    {
+      const auto& path_pose = preserved_path.poses[i].pose;
+      EXPECT_NEAR(path_pose.position.x, 0.1f * (i + 1), 1.0e-6);
+      EXPECT_NEAR(path_pose.position.y, 0.0, 1.0e-6);
+      EXPECT_NEAR(tf2::getYaw(path_pose.orientation), 0.0, 1.0e-6);
+    }
+    EXPECT_NEAR(predictor_.getPreservedPathLength(), 0.9, 1.0e-6);
+  }
+  {
+    StartPosePredictor::Astar::Vec start_grid;
+    EXPECT_TRUE(predictor_.process(getPose(-0.05, 0.05, 0.0), cm_, map_info_, paths_["straight"].second, start_grid));
+    EXPECT_EQ(StartPosePredictor::Astar::Vec(6, 5, 0), start_grid);
+    const auto preserved_path = predictor_.getPreservedPath();
+    // No points are removed as the robot have not reached the first line strip.
+    ASSERT_EQ(preserved_path.poses.size(), 10);
+    for (size_t i = 0; i < preserved_path.poses.size(); ++i)
+    {
+      const auto& path_pose = preserved_path.poses[i].pose;
+      EXPECT_NEAR(path_pose.position.x, 0.1f * i, 1.0e-6);
+      EXPECT_NEAR(path_pose.position.y, 0.0, 1.0e-6);
+      EXPECT_NEAR(tf2::getYaw(path_pose.orientation), 0.0, 1.0e-6);
+    }
+    EXPECT_NEAR(predictor_.getPreservedPathLength(), 1.0, 1.0e-6);
+  }
+
+  {
+    // The robot will collide with an obstacle during the planning.
+    cm_[GridAstar<3, 2>::Vec(6, 5, 0)] = 100;
+
+    StartPosePredictor::Astar::Vec start_grid;
+    EXPECT_FALSE(predictor_.process(getPose(0.05, 0.05, 0.0), cm_, map_info_, paths_["straight"].second, start_grid));
+    EXPECT_TRUE(predictor_.getPreservedPath().poses.empty());
+    EXPECT_EQ(predictor_.getPreservedPathLength(), 0.0);
+  }
+}
+
+TEST_F(StartPosePredictorTester, SwitchBack)
+{
+  {
+    StartPosePredictor::Astar::Vec start_grid;
+    EXPECT_TRUE(predictor_.process(getPose(0.25, 0.0, 0.0), cm_, map_info_, paths_["switch_back"].second, start_grid));
+    // The robot will not reach the switch back within 2.0 seconds, and the start grid will be on the first line.
+    EXPECT_EQ(StartPosePredictor::Astar::Vec(7, 5, 0), start_grid);
+    const auto preserved_path = predictor_.getPreservedPath();
+    ASSERT_EQ(preserved_path.poses.size(), 17);
+    for (size_t i = 0; i < preserved_path.poses.size(); ++i)
+    {
+      const auto& path_pose = preserved_path.poses[i].pose;
+      const auto& expected_path_pose = paths_["switch_back"].second.poses[i + 3].pose;
+      EXPECT_NEAR(path_pose.position.x, expected_path_pose.position.x, 1.0e-6);
+      EXPECT_NEAR(path_pose.position.y, expected_path_pose.position.y, 1.0e-6);
+      EXPECT_NEAR(tf2::getYaw(path_pose.orientation), tf2::getYaw(expected_path_pose.orientation), 1.0e-6);
+    }
+  }
+
+  {
+    StartPosePredictor::Astar::Vec start_grid;
+    EXPECT_TRUE(predictor_.process(getPose(0.55, 0.0, 0.0), cm_, map_info_, paths_["switch_back"].second, start_grid));
+    // The robot will reach the switch back within 2.0 seconds, and the start grid is the switch back point.
+    EXPECT_EQ(StartPosePredictor::Astar::Vec(8, 6, 1), start_grid);
+    const auto preserved_path = predictor_.getPreservedPath();
+    // A part of the first forward, the second forward, and the first turn.
+    ASSERT_EQ(preserved_path.poses.size(), 5 + 10 + 13);
+    for (size_t i = 0; i < preserved_path.poses.size(); ++i)
+    {
+      const auto& path_pose = preserved_path.poses[i].pose;
+      const auto& expected_path_pose = paths_["switch_back"].second.poses[i + 6].pose;
+      EXPECT_NEAR(path_pose.position.x, expected_path_pose.position.x, 1.0e-6);
+      EXPECT_NEAR(path_pose.position.y, expected_path_pose.position.y, 1.0e-6);
+      EXPECT_NEAR(tf2::getYaw(path_pose.orientation), tf2::getYaw(expected_path_pose.orientation), 1.0e-6);
+    }
+  }
+}
+
+}  // namespace planner_3d
+}  // namespace planner_cspace
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  return RUN_ALL_TESTS();
+}
diff --git a/planner_cspace/test/test/dynamic_parameter_change_rostest.test b/planner_cspace/test/test/dynamic_parameter_change_rostest.test
index 1f8e18f6..13078882 100644
--- a/planner_cspace/test/test/dynamic_parameter_change_rostest.test
+++ b/planner_cspace/test/test/dynamic_parameter_change_rostest.test
@@ -1,7 +1,21 @@
 <?xml version="1.0"?>
 <launch>
   <env name="GCOV_PREFIX" value="/tmp/gcov/dynamic_parameter_change" />
+  <param name="neonavigation_compatible" value="1" />
 
   <test test-name="test_dynamic_parameter_change" pkg="planner_cspace" type="test_dynamic_parameter_change" time-limit="90.0" />
-  <include file="$(find planner_cspace)/test/test/actionlib_common_rostest.test" />
+
+  <node pkg="costmap_cspace" type="costmap_3d" name="costmap_3d">
+    <rosparam param="footprint">[[0.01, -0.01], [0.01, 0.01], [-0.01, 0.01], [-0.01, -0.01]]</rosparam>
+    <param name="ang_resolution" value="16"/>
+    <param name="linear_expand" value="0.0"/>
+    <param name="linear_spread" value="0.3"/>
+  </node>
+  <node pkg="planner_cspace" type="planner_3d" name="planner_3d">
+    <remap from="move_base_simple/goal" to="/goal" />
+  </node>
+  <node pkg="map_server" type="map_server" name="map_server_global" args="$(find planner_cspace)/test/data/global_map.yaml" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="stf1"
+        args="0 0 0 0 0 0 map odom" />
+
 </launch>
diff --git a/trajectory_tracker/include/trajectory_tracker/path2d.h b/trajectory_tracker/include/trajectory_tracker/path2d.h
index 9e90a34b..298a4a14 100644
--- a/trajectory_tracker/include/trajectory_tracker/path2d.h
+++ b/trajectory_tracker/include/trajectory_tracker/path2d.h
@@ -45,6 +45,8 @@
 #include <trajectory_tracker/average.h>
 #include <trajectory_tracker/eigen_line.h>
 
+#include <ros/ros.h>
+
 namespace trajectory_tracker
 {
 class Pose2D
@@ -132,24 +134,36 @@ class Path2D : public std::vector<Pose2D>
   inline ConstIterator findLocalGoal(
       const ConstIterator& begin,
       const ConstIterator& end,
-      const bool allow_backward_motion) const
+      const bool allow_switch_back,
+      const bool allow_in_place_turn = true,
+      const double epsilon = 1e-6) const
   {
     float sign_vel_prev = 0;
     ConstIterator it_prev = begin;
     for (ConstIterator it = begin + 1; it < end; ++it)
     {
-      // stop reading forward if the path is switching back or in-place turning
-      if (it->pos_ == it_prev->pos_)
-        return it;
-
-      const Eigen::Vector2d inc = it->pos_ - it_prev->pos_;
-
-      const float angle = atan2(inc[1], inc[0]);
-      const float angle_pose = allow_backward_motion ? it->yaw_ : angle;
-      const float sign_vel_req = std::cos(angle) * std::cos(angle_pose) + std::sin(angle) * std::sin(angle_pose);
-      if (sign_vel_prev * sign_vel_req < 0)
-        return it;
-      sign_vel_prev = sign_vel_req;
+      if ((it->pos_ - it_prev->pos_).squaredNorm() < epsilon)
+      {
+        if (allow_in_place_turn)
+        {
+          // stop reading forward if the path is in-place turning
+          return it;
+        }
+        sign_vel_prev = 0;
+      }
+      else if (allow_switch_back)
+      {
+        const Eigen::Vector2d inc = it->pos_ - it_prev->pos_;
+        const float angle = atan2(inc[1], inc[0]);
+        const float angle_pose = it->yaw_;
+        const float sign_vel_req = std::cos(angle) * std::cos(angle_pose) + std::sin(angle) * std::sin(angle_pose);
+        if (sign_vel_prev * sign_vel_req < 0)
+        {
+          // stop reading forward if the path is switching back
+          return it;
+        }
+        sign_vel_prev = sign_vel_req;
+      }
       it_prev = it;
     }
     return end;
@@ -335,6 +349,63 @@ class Path2D : public std::vector<Pose2D>
       at(i).toMsg(path.poses[i]);
     }
   }
+
+  inline std::vector<ConstIterator> enumerateLocalGoals(
+      const ConstIterator& begin,
+      const ConstIterator& end,
+      const bool allow_switch_back,
+      const bool allow_in_place_turn = true,
+      const double epsilon = 1e-6) const
+  {
+    ConstIterator it_search_begin = begin;
+    std::vector<ConstIterator> results;
+    while (true)
+    {
+      const ConstIterator it_local_goal =
+          findLocalGoal(it_search_begin, end, allow_switch_back, allow_in_place_turn, epsilon);
+      if (it_local_goal == end)
+        break;
+      results.push_back(it_local_goal);
+      it_search_begin = it_local_goal;
+    }
+    return results;
+  }
+
+  inline std::vector<double> getEstimatedTimeOfArrivals(
+      const ConstIterator& begin,
+      const ConstIterator& end,
+      const double linear_speed,
+      const double angular_speed,
+      const double initial_eta_sec = 0.0) const
+  {
+    if (begin == end)
+    {
+      return std::vector<double>();
+    }
+    std::vector<double> results(1, initial_eta_sec);
+    double elapsed_sec = initial_eta_sec;
+    ConstIterator it_prev = begin;
+    for (ConstIterator it = begin + 1; it < end; ++it)
+    {
+      const double dist = (it_prev->pos_ - it->pos_).norm();
+      if (dist > 1.0e-6)
+      {
+        elapsed_sec += dist / linear_speed;
+      }
+      else
+      {
+        double ang_diff = std::abs(it_prev->yaw_ - it->yaw_);
+        if (ang_diff > M_PI)
+        {
+          ang_diff = 2 * M_PI - ang_diff;
+        }
+        elapsed_sec += ang_diff / angular_speed;
+      }
+      results.push_back(elapsed_sec);
+      it_prev = it;
+    }
+    return results;
+  }
 };
 }  // namespace trajectory_tracker
 
diff --git a/trajectory_tracker/test/src/test_path2d.cpp b/trajectory_tracker/test/src/test_path2d.cpp
index 411f463c..1a989b16 100644
--- a/trajectory_tracker/test/src/test_path2d.cpp
+++ b/trajectory_tracker/test/src/test_path2d.cpp
@@ -140,15 +140,73 @@ TEST(Path2D, LocalGoalWithSwitchBack)
       yaw += yaw_diff;
       path.push_back(trajectory_tracker::Pose2D(p, yaw, 1));
     }
-    const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true);
-    ASSERT_EQ(it_local_goal, path.begin() + 5);
-    ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true), path.end());
 
+    {
+      const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, true);
+      ASSERT_EQ(it_local_goal, path.begin() + 5);
+      ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, true), path.end());
+    }
+    {
+      const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, false);
+      ASSERT_EQ(it_local_goal, path.begin() + 5);
+      ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, false), path.end());
+    }
     // no switch back motion under (allow_switchback == false)
-    ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), false), path.end());
+    ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), false, true), path.end());
+  }
+}
+
+TEST(Path2D, LocalGoalTest)
+{
+  trajectory_tracker::Path2D path;
+  Eigen::Vector2d p(0, 0);
+  for (size_t i = 0; i <= 10; ++i)
+  {
+    const double yaw = M_PI / 2 / 10;
+    p.x() = std::sin(i * yaw);
+    p.y() = 1.0 - std::cos(i * yaw);
+    path.push_back(trajectory_tracker::Pose2D(p, yaw, 0));
+  }
+  // Turn in-place and go back
+  for (size_t i = 0; i <= 10; ++i)
+  {
+    path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(1.0 - i * 0.1, 1.0), 0, 0));
+  }
+  {
+    const auto it_local_goal = path.findLocalGoal(path.begin(), path.end(), true, true);
+    ASSERT_EQ(it_local_goal, path.begin() + 11);
+    ASSERT_EQ(path.findLocalGoal(it_local_goal, path.end(), true, true), path.end());
+  }
+  {
+    ASSERT_EQ(path.findLocalGoal(path.begin(), path.end(), true, false), path.end());
   }
 }
 
+TEST(Path2D, EnumerateLocalGoals)
+{
+  trajectory_tracker::Path2D path;
+  const std::vector<float> yaws = {0.5, -0.2, 1.0};
+  double x = 0.0;
+  double y = 0.0;
+  for (const float yaw : yaws)
+  {
+    for (size_t i = 0; i < 10; ++i)
+    {
+      path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), yaw, 1.0));
+      x += std::cos(yaw) * 0.1;
+      y += std::sin(yaw) * 0.1;
+    }
+    path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), yaw, 1.0));
+  }
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(x, y), 0.3, 1.0));
+
+  const auto local_goals = path.enumerateLocalGoals(path.begin(), path.end(), true);
+  ASSERT_EQ(local_goals.size(), 3);
+  ASSERT_EQ(local_goals.at(0) - path.begin(), 11);
+  ASSERT_EQ(local_goals.at(1) - path.begin(), 22);
+  ASSERT_EQ(local_goals.at(2) - path.begin(), 33);
+}
+
 TEST(Path2D, FindNearestWithDistance)
 {
   trajectory_tracker::Path2D path;
@@ -291,6 +349,42 @@ TEST(Path2D, Conversions)
   }
 }
 
+TEST(Path2D, EstimatedTimeOfArrivals)
+{
+  trajectory_tracker::Path2D path;
+  // Straight
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.0, 0.0), 135.0 / 180.0 * M_PI, 0));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.1, -0.1), 135.0 / 180.0 * M_PI, 0));
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.2, -0.2), 135.0 / 180.0 * M_PI, 0));
+  // In-place-turn
+  path.push_back(trajectory_tracker::Pose2D(Eigen::Vector2d(0.2, -0.2), -M_PI / 2, 0));
+  // Curve
+  for (int i = 1; i <= 4; ++i)
+  {
+    const double angle = M_PI / 8 * i;
+    path.push_back(trajectory_tracker::Pose2D(
+        Eigen::Vector2d(0.2 * std::cos(angle), -0.2 + 0.2 * std::sin(angle)), -M_PI / 2 - angle, 0));
+  }
+  const double linear_speed = 0.5;
+  const double angular_speed = M_PI;
+  const std::vector<double> etas =
+      path.getEstimatedTimeOfArrivals(path.begin(), path.end(), linear_speed, angular_speed, 1.0);
+  ASSERT_EQ(etas.size(), 8);
+  double expected_eta = 1.0;
+  EXPECT_NEAR(etas[0], expected_eta, 1.0e-6);
+  expected_eta += std::hypot(0.1, 0.1) / linear_speed;
+  EXPECT_NEAR(etas[1], expected_eta, 1.0e-6);
+  expected_eta += std::hypot(0.1, 0.1) / linear_speed;
+  EXPECT_NEAR(etas[2], expected_eta, 1.0e-6);
+  expected_eta += 135.0 / (180.0 / M_PI) / angular_speed;
+  EXPECT_NEAR(etas[3], expected_eta, 1.0e-6);
+  const double turn_dist = std::hypot(0.2 - 0.2 * std::cos(M_PI / 8), 0.2 * std::sin(M_PI / 8));
+  for (size_t p = 4; p < etas.size(); ++p)
+  {
+    EXPECT_NEAR(etas[p], expected_eta + (p - 3) * turn_dist / linear_speed, 1.0e-6);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);