diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
index b147b7e665c..b91a70ccb23 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp
@@ -73,16 +73,6 @@ class PathLongerOnApproach : public BT::DecoratorNode
   BT::NodeStatus tick() override;
 
 private:
-  /**
-   * @brief Checks if the global path is updated
-   * @param new_path new path to the goal
-   * @param old_path current path to the goal
-   * @return whether the path is updated for the current goal
-   */
-  bool isPathUpdated(
-    nav_msgs::msg::Path & new_path,
-    nav_msgs::msg::Path & old_path);
-
   /**
    * @brief Checks if the robot is in the goal proximity
    * @param old_path current path to the goal
diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp
index aee651690e3..ecd8112e4fc 100644
--- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp
+++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp
@@ -16,6 +16,7 @@
 #include <memory>
 #include <vector>
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/path_utils.hpp"
 
 #include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
 
@@ -30,16 +31,6 @@ PathLongerOnApproach::PathLongerOnApproach(
   node_ = config().blackboard->get<nav2::LifecycleNode::SharedPtr>("node");
 }
 
-bool PathLongerOnApproach::isPathUpdated(
-  nav_msgs::msg::Path & new_path,
-  nav_msgs::msg::Path & old_path)
-{
-  return old_path.poses.size() != 0 &&
-         new_path.poses.size() != 0 &&
-         new_path.poses.size() != old_path.poses.size() &&
-         old_path.poses.back().pose.position == new_path.poses.back().pose.position;
-}
-
 bool PathLongerOnApproach::isRobotInGoalProximity(
   nav_msgs::msg::Path & old_path,
   double & prox_leng)
@@ -74,7 +65,8 @@ inline BT::NodeStatus PathLongerOnApproach::tick()
 
   // Check if the path is updated and valid, compare the old and the new path length,
   // given the goal proximity and check if the new path is longer
-  if (isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_, prox_len_) &&
+  if (nav2_util::isPathUpdated(new_path_, old_path_) && isRobotInGoalProximity(old_path_,
+    prox_len_) &&
     isNewPathLonger(new_path_, old_path_, length_factor_) && !first_time_)
   {
     const BT::NodeStatus child_state = child_node_->executeTick();
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index a21450f08f3..269d7b34882 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -55,10 +55,12 @@ bt_navigator:
       plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
       enable_groot_monitoring: false
       groot_server_port: 1667
+      search_window: 2.0
     navigate_through_poses:
       plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
       enable_groot_monitoring: false
       groot_server_port: 1669
+      search_window: 2.0
     bt_search_directories:
       - $(find-pkg-share nav2_bt_navigator)/behavior_trees
     # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
index 427373ded47..f6ace917787 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp
@@ -110,10 +110,14 @@ class NavigateThroughPosesNavigator
   rclcpp::Time start_time_;
   std::string goals_blackboard_id_;
   std::string path_blackboard_id_;
+  std::string tracking_feedback_blackboard_id_;
   std::string waypoint_statuses_blackboard_id_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+  size_t start_index_ = 0;
+  nav_msgs::msg::Path previous_path_;
+  double search_window_;
 };
 
 }  // namespace nav2_bt_navigator
diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
index f63c27844dc..2ebe7b0958a 100644
--- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
+++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp
@@ -127,9 +127,13 @@ class NavigateToPoseNavigator
 
   std::string goal_blackboard_id_;
   std::string path_blackboard_id_;
+  std::string tracking_feedback_blackboard_id_;
 
   // Odometry smoother object
   std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
+  size_t start_index_ = 0;
+  nav_msgs::msg::Path previous_path_;
+  double search_window_;
 };
 
 }  // namespace nav2_bt_navigator
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
index 1144ab3d8b0..3717d86cc79 100644
--- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -19,6 +19,8 @@
 #include <limits>
 #include <stdexcept>
 #include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
+#include "nav2_util/path_utils.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -35,11 +37,16 @@ NavigateThroughPosesNavigator::configure(
     node->declare_or_get_parameter(getName() + ".goals_blackboard_id", std::string("goals"));
   path_blackboard_id_ =
     node->declare_or_get_parameter(getName() + ".path_blackboard_id", std::string("path"));
+  tracking_feedback_blackboard_id_ = node->declare_or_get_parameter(
+    getName() + ".tracking_feedback_blackboard_id",
+    std::string("tracking_feedback"));
   waypoint_statuses_blackboard_id_ =
     node->declare_or_get_parameter(
     getName() + ".waypoint_statuses_blackboard_id",
     std::string("waypoint_statuses"));
 
+  search_window_ = node->declare_or_get_parameter(getName() + "search_window", 2.0);
+
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
 
@@ -157,25 +164,21 @@ NavigateThroughPosesNavigator::onLoop()
   nav_msgs::msg::Path current_path;
   res = blackboard->get(path_blackboard_id_, current_path);
   if (res && current_path.poses.size() > 0u) {
+    // Reset start index if path is updated
+    if (nav2_util::isPathUpdated(current_path,
+        previous_path_) || previous_path_.poses.size() == 0u)
+    {
+      start_index_ = 0;
+      previous_path_ = current_path;
+    }
     // Find the closest pose to current pose on global path
-    auto find_closest_pose_idx =
-      [&current_pose, &current_path]() {
-        size_t closest_pose_idx = 0;
-        double curr_min_dist = std::numeric_limits<double>::max();
-        for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
-          double curr_dist = nav2_util::geometry_utils::euclidean_distance(
-            current_pose, current_path.poses[curr_idx]);
-          if (curr_dist < curr_min_dist) {
-            curr_min_dist = curr_dist;
-            closest_pose_idx = curr_idx;
-          }
-        }
-        return closest_pose_idx;
-      };
+    const auto path_search_result = nav2_util::distance_from_path(
+      current_path, current_pose.pose, start_index_, search_window_);
 
     // Calculate distance on the path
+    start_index_ = path_search_result.closest_segment_index;
     double distance_remaining =
-      nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
+      nav2_util::geometry_utils::calculate_path_length(current_path, start_index_);
 
     // Default value for time remaining
     rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
@@ -201,6 +204,11 @@ NavigateThroughPosesNavigator::onLoop()
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;
   feedback_msg->number_of_poses_remaining = goal_poses.goals.size();
+  nav2_msgs::msg::TrackingFeedback tracking_feedback;
+  res = blackboard->get(
+    tracking_feedback_blackboard_id_,
+    tracking_feedback);
+  feedback_msg->tracking_error = tracking_feedback.tracking_error;
 
   bt_action_server_->publishFeedback(feedback_msg);
 }
@@ -279,6 +287,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
   start_time_ = clock_->now();
   auto blackboard = bt_action_server_->getBlackboard();
   blackboard->set("number_recoveries", 0);  // NOLINT
+  previous_path_ = nav_msgs::msg::Path();
 
   // Update the goal pose on the blackboard
   blackboard->set<nav_msgs::msg::Goals>(
diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
index 73989dd91de..19c0984087f 100644
--- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
+++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp
@@ -17,6 +17,8 @@
 #include <memory>
 #include <limits>
 #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
+#include "nav2_util/path_utils.hpp"
+#include "nav2_msgs/msg/tracking_feedback.hpp"
 
 namespace nav2_bt_navigator
 {
@@ -35,6 +37,11 @@ NavigateToPoseNavigator::configure(
   path_blackboard_id_ = node->declare_or_get_parameter(
     getName() + ".path_blackboard_id",
     std::string("path"));
+  tracking_feedback_blackboard_id_ = node->declare_or_get_parameter(
+    getName() + ".tracking_feedback_blackboard_id",
+    std::string("tracking_feedback"));
+
+  search_window_ = node->declare_or_get_parameter(getName() + "search_window", 2.0);
 
   // Odometry smoother object for getting current speed
   odom_smoother_ = odom_smoother;
@@ -138,25 +145,21 @@ NavigateToPoseNavigator::onLoop()
   nav_msgs::msg::Path current_path;
   auto res = blackboard->get(path_blackboard_id_, current_path);
   if (res && current_path.poses.size() > 0u) {
+    // Reset start index if path is updated
+    if (nav2_util::isPathUpdated(current_path,
+        previous_path_) || previous_path_.poses.size() == 0u)
+    {
+      start_index_ = 0;
+      previous_path_ = current_path;
+    }
     // Find the closest pose to current pose on global path
-    auto find_closest_pose_idx =
-      [&current_pose, &current_path]() {
-        size_t closest_pose_idx = 0;
-        double curr_min_dist = std::numeric_limits<double>::max();
-        for (size_t curr_idx = 0; curr_idx < current_path.poses.size(); ++curr_idx) {
-          double curr_dist = nav2_util::geometry_utils::euclidean_distance(
-            current_pose, current_path.poses[curr_idx]);
-          if (curr_dist < curr_min_dist) {
-            curr_min_dist = curr_dist;
-            closest_pose_idx = curr_idx;
-          }
-        }
-        return closest_pose_idx;
-      };
+    const auto path_search_result = nav2_util::distance_from_path(
+      current_path, current_pose.pose, start_index_, search_window_);
 
     // Calculate distance on the path
+    start_index_ = path_search_result.closest_segment_index;
     double distance_remaining =
-      nav2_util::geometry_utils::calculate_path_length(current_path, find_closest_pose_idx());
+      nav2_util::geometry_utils::calculate_path_length(current_path, start_index_);
 
     // Default value for time remaining
     rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds(0.0);
@@ -181,6 +184,11 @@ NavigateToPoseNavigator::onLoop()
   feedback_msg->number_of_recoveries = recovery_count;
   feedback_msg->current_pose = current_pose;
   feedback_msg->navigation_time = clock_->now() - start_time_;
+  nav2_msgs::msg::TrackingFeedback tracking_feedback;
+  res = blackboard->get(
+    tracking_feedback_blackboard_id_,
+    tracking_feedback);
+  feedback_msg->tracking_error = tracking_feedback.tracking_error;
 
   bt_action_server_->publishFeedback(feedback_msg);
 }
@@ -255,6 +263,7 @@ NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
   start_time_ = clock_->now();
   auto blackboard = bt_action_server_->getBlackboard();
   blackboard->set("number_recoveries", 0);  // NOLINT
+  previous_path_ = nav_msgs::msg::Path();
 
   // Update the goal pose on the blackboard
   blackboard->set(goal_blackboard_id_, goal_pose);
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index 7f9765350f8..2545457df2b 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -744,9 +744,9 @@ void ControllerServer::computeAndPublishVelocity()
     tracking_feedback_msg->robot_pose = pose;
     tracking_feedback_msg->distance_to_goal = current_distance_to_goal;
     tracking_feedback_msg->speed = std::hypot(twist.linear.x, twist.linear.y);
+    start_index_ = path_search_result.closest_segment_index;
     tracking_feedback_msg->remaining_path_length =
       nav2_util::geometry_utils::calculate_path_length(current_path_, start_index_);
-    start_index_ = path_search_result.closest_segment_index;
 
     // Update current tracking error and publish
     current_tracking_feedback = *tracking_feedback_msg;
diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action
index 1d7a1951578..97f63a2cdf7 100644
--- a/nav2_msgs/action/NavigateThroughPoses.action
+++ b/nav2_msgs/action/NavigateThroughPoses.action
@@ -23,5 +23,6 @@ builtin_interfaces/Duration navigation_time
 builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining
+float32 tracking_error
 int16 number_of_poses_remaining
 WaypointStatus[] waypoint_statuses
diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action
index 14272c5663a..ee886672115 100644
--- a/nav2_msgs/action/NavigateToPose.action
+++ b/nav2_msgs/action/NavigateToPose.action
@@ -21,3 +21,4 @@ builtin_interfaces/Duration navigation_time
 builtin_interfaces/Duration estimated_time_remaining
 int16 number_of_recoveries
 float32 distance_remaining
+float32 tracking_error
diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp
index ee4177c5208..da50c2e66e0 100644
--- a/nav2_rviz_plugins/src/nav2_panel.cpp
+++ b/nav2_rviz_plugins/src/nav2_panel.cpp
@@ -1084,6 +1084,9 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
       syncTabsWithAccumulatedPoses();  // Sync tabs with new pose
     } else {
       acummulated_poses_ = nav_msgs::msg::Goals();
+      // Reset goal index for single goal navigation
+      goal_index_ = 0;
+      store_poses_.goals.push_back(pose);
       updateWpNavigationMarkers();
       std::cout << "Start navigation" << std::endl;
       startNavigation(pose);
@@ -1627,6 +1630,8 @@ inline std::string Nav2Panel::toLabel(T & msg)
     toString(rclcpp::Duration(msg.estimated_time_remaining).seconds(), 0) + " s"
     "</td></tr><tr><td width=150>Distance remaining:</td><td>" +
     toString(msg.distance_remaining, 2) + " m"
+    "</td></tr><tr><td width=150>Tracking error:</td><td>" +
+    toString(msg.tracking_error, 2) + " m"
     "</td></tr><tr><td width=150>Time taken:</td><td>" +
     toString(rclcpp::Duration(msg.navigation_time).seconds(), 0) + " s"
     "</td></tr><tr><td width=150>Recoveries:</td><td>" +
diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
index 45bf3fd4e77..ca165e392a7 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py
@@ -109,6 +109,18 @@ def main() -> None:
                 + ' seconds.'
             )
 
+            print(
+                'Distance remaining: '
+                + '{:.2f}'.format(feedback.distance_remaining)
+                + ' meters.'
+            )
+
+            print(
+                'Tracking error: '
+                + '{:.2f}'.format(feedback.tracking_error)
+                + ' meters.'
+            )
+
             # Some navigation timeout to demo cancellation
             if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
                 navigator.cancelTask()
diff --git a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
index 76855435e8d..8e86a9c41c8 100644
--- a/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
+++ b/nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py
@@ -89,6 +89,18 @@ def main() -> None:
                 + ' seconds.'
             )
 
+            print(
+                'Distance remaining: '
+                + '{:.2f}'.format(feedback.distance_remaining)
+                + ' meters.'
+            )
+
+            print(
+                'Tracking error: '
+                + '{:.2f}'.format(feedback.tracking_error)
+                + ' meters.'
+            )
+
             # Some navigation timeout to demo cancellation
             if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
                 navigator.cancelTask()
diff --git a/nav2_util/include/nav2_util/path_utils.hpp b/nav2_util/include/nav2_util/path_utils.hpp
index 11a51d850f6..21594c729b8 100644
--- a/nav2_util/include/nav2_util/path_utils.hpp
+++ b/nav2_util/include/nav2_util/path_utils.hpp
@@ -100,6 +100,16 @@ unsigned int removePosesAfterFirstConstraint(
   bool enforce_path_inversion,
   float rotation_threshold);
 
+/**
+ * @brief Checks if the global path is updated
+ * @param new_path new path to the goal
+ * @param old_path current path to the goal
+ * @return whether the path is updated for the current goal
+ */
+bool isPathUpdated(
+  nav_msgs::msg::Path & new_path,
+  nav_msgs::msg::Path & old_path);
+
 }  // namespace nav2_util
 
 #endif  // NAV2_UTIL__PATH_UTILS_HPP_
diff --git a/nav2_util/src/path_utils.cpp b/nav2_util/src/path_utils.cpp
index 17e3f59a5b3..5afa1599b1d 100644
--- a/nav2_util/src/path_utils.cpp
+++ b/nav2_util/src/path_utils.cpp
@@ -219,4 +219,14 @@ unsigned int removePosesAfterFirstConstraint(
   return first_after_constraint;
 }
 
+bool isPathUpdated(
+  nav_msgs::msg::Path & new_path,
+  nav_msgs::msg::Path & old_path)
+{
+  return old_path.poses.size() != 0 &&
+         new_path.poses.size() != 0 &&
+         new_path.poses.size() != old_path.poses.size() &&
+         old_path.poses.back().pose.position == new_path.poses.back().pose.position;
+}
+
 }  // namespace nav2_util
diff --git a/nav2_util/test/test_path_utils.cpp b/nav2_util/test/test_path_utils.cpp
index 2b0c808393a..07c021ca22e 100644
--- a/nav2_util/test/test_path_utils.cpp
+++ b/nav2_util/test/test_path_utils.cpp
@@ -598,3 +598,32 @@ TEST(UtilsTests, RemovePosesAfterPathInversionTest)
   EXPECT_EQ(path.poses.size(), 11u);
   EXPECT_EQ(path.poses.back().pose.position.x, 10);
 }
+
+TEST(UtilsTests, IsPathUpdatedTest)
+{
+  auto makePose = [](double x, double y) {
+      geometry_msgs::msg::PoseStamped pose;
+      pose.pose.position.x = x;
+      pose.pose.position.y = y;
+      pose.pose.position.z = 0.0;
+      return pose;
+    };
+
+  // Same end pose but different size
+  nav_msgs::msg::Path old_path, new_path;
+  old_path.poses.push_back(makePose(0.0, 0.0));
+  old_path.poses.push_back(makePose(1.0, 1.0));
+  new_path.poses.push_back(makePose(0.0, 0.0));
+  new_path.poses.push_back(makePose(0.5, 0.5));
+  new_path.poses.push_back(makePose(1.0, 1.0));
+  EXPECT_TRUE(nav2_util::isPathUpdated(old_path, new_path));
+
+  // Same size and same end pose
+  old_path.poses.clear();
+  new_path.poses.clear();
+  old_path.poses.push_back(makePose(0.0, 0.0));
+  old_path.poses.push_back(makePose(1.0, 1.0));
+  new_path.poses.push_back(makePose(0.0, 0.0));
+  new_path.poses.push_back(makePose(1.0, 1.0));
+  EXPECT_FALSE(nav2_util::isPathUpdated(old_path, new_path));
+}