diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
index 00503d4d293..b4a65be8e48 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp
@@ -27,16 +27,6 @@
 namespace nav2_smac_planner
 {
 
-/**
- * @class nav2_smac_planner::PathSegment
- * @brief A segment of a path in start/end indices
- */
-struct PathSegment
-{
-  unsigned int start;
-  unsigned int end;
-};
-
 /**
  * @struct nav2_smac_planner::BoundaryPoints
  * @brief Set of boundary condition points from expansion
@@ -144,14 +134,6 @@ class Smoother
     geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
     const double & value);
 
-  /**
-   * @brief Finds the starting and end indices of path segments where
-   * the robot is traveling in the same direction (e.g. forward vs reverse)
-   * @param path Path in which to look for cusps
-   * @return Set of index pairs for each segment of the path in a given direction
-   */
-  std::vector<PathSegment> findDirectionalPathSegments(const nav_msgs::msg::Path & path);
-
   /**
    * @brief Enforced minimum curvature boundary conditions on plan output
    * the robot is traveling in the same direction (e.g. forward vs reverse)
@@ -214,15 +196,6 @@ class Smoother
   template<typename IteratorT>
   BoundaryExpansions generateBoundaryExpansionPoints(IteratorT start, IteratorT end);
 
-  /**
-   * @brief For a given path, update the path point orientations based on smoothing
-   * @param path Path to approximate the path orientation in
-   * @param reversing_segment Return if this is a reversing segment
-   */
-  inline void updateApproximatePathOrientations(
-    nav_msgs::msg::Path & path,
-    bool & reversing_segment);
-
   double min_turning_rad_, tolerance_, data_w_, smooth_w_;
   int max_its_, refinement_ctr_, refinement_num_;
   bool is_holonomic_, do_refinement_;
diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp
index be417fa1be7..456f26a2566 100644
--- a/nav2_smac_planner/src/smoother.cpp
+++ b/nav2_smac_planner/src/smoother.cpp
@@ -24,11 +24,13 @@
 #include "tf2/utils.hpp"
 
 #include "nav2_smac_planner/smoother.hpp"
+#include "nav2_util/smoother_utils.hpp"
 
 namespace nav2_smac_planner
 {
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
+using nav2_util::PathSegment;
 
 Smoother::Smoother(const SmootherParams & params)
 {
@@ -62,7 +64,8 @@ bool Smoother::smooth(
   bool success = true, reversing_segment;
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
-  std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
+  std::vector<PathSegment> path_segments = nav2_util::findDirectionalPathSegments(path,
+      is_holonomic_);
 
   for (unsigned int i = 0; i != path_segments.size(); i++) {
     if (path_segments[i].end - path_segments[i].start > 10) {
@@ -130,7 +133,7 @@ bool Smoother::smoothImpl(
         rclcpp::get_logger("SmacPlannerSmoother"),
         "Number of iterations has exceeded limit of %i.", max_its_);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
       return false;
     }
 
@@ -142,7 +145,7 @@ bool Smoother::smoothImpl(
         rclcpp::get_logger("SmacPlannerSmoother"),
         "Smoothing time exceeded allowed duration of %0.2f.", max_time);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
       return false;
     }
 
@@ -176,7 +179,7 @@ bool Smoother::smoothImpl(
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced.");
         path = last_path;
-        updateApproximatePathOrientations(path, reversing_segment);
+        nav2_util::updateApproximatePathOrientations(path, reversing_segment, is_holonomic_);
         return false;
       }
     }
@@ -191,7 +194,7 @@ bool Smoother::smoothImpl(
     smoothImpl(new_path, reversing_segment, costmap, max_time);
   }
 
-  updateApproximatePathOrientations(new_path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(new_path, reversing_segment, is_holonomic_);
   path = new_path;
   return true;
 }
@@ -221,92 +224,6 @@ void Smoother::setFieldByDim(
   }
 }
 
-std::vector<PathSegment> Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path)
-{
-  std::vector<PathSegment> segments;
-  PathSegment curr_segment;
-  curr_segment.start = 0;
-
-  // If holonomic, no directional changes and
-  // may have abrupt angular changes from naive grid search
-  if (is_holonomic_) {
-    curr_segment.end = path.poses.size() - 1;
-    segments.push_back(curr_segment);
-    return segments;
-  }
-
-  // Iterating through the path to determine the position of the cusp
-  for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
-    // We have two vectors for the dot product OA and AB. Determining the vectors.
-    double oa_x = path.poses[idx].pose.position.x -
-      path.poses[idx - 1].pose.position.x;
-    double oa_y = path.poses[idx].pose.position.y -
-      path.poses[idx - 1].pose.position.y;
-    double ab_x = path.poses[idx + 1].pose.position.x -
-      path.poses[idx].pose.position.x;
-    double ab_y = path.poses[idx + 1].pose.position.y -
-      path.poses[idx].pose.position.y;
-
-    // Checking for the existence of cusp, in the path, using the dot product.
-    double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
-    if (dot_product < 0.0) {
-      curr_segment.end = idx;
-      segments.push_back(curr_segment);
-      curr_segment.start = idx;
-    }
-
-    // Checking for the existence of a differential rotation in place.
-    double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
-    double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
-    double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
-    if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
-      curr_segment.end = idx;
-      segments.push_back(curr_segment);
-      curr_segment.start = idx;
-    }
-  }
-
-  curr_segment.end = path.poses.size() - 1;
-  segments.push_back(curr_segment);
-  return segments;
-}
-
-void Smoother::updateApproximatePathOrientations(
-  nav_msgs::msg::Path & path,
-  bool & reversing_segment)
-{
-  double dx, dy, theta, pt_yaw;
-  reversing_segment = false;
-
-  // Find if this path segment is in reverse
-  dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
-  dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
-  theta = atan2(dy, dx);
-  pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
-  if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
-    reversing_segment = true;
-  }
-
-  // Find the angle relative the path position vectors
-  for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
-    dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
-    dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
-    theta = atan2(dy, dx);
-
-    // If points are overlapping, pass
-    if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
-      continue;
-    }
-
-    // Flip the angle if this path segment is in reverse
-    if (reversing_segment) {
-      theta += M_PI;  // orientationAroundZAxis will normalize
-    }
-
-    path.poses[i].pose.orientation = orientationAroundZAxis(theta);
-  }
-}
-
 unsigned int Smoother::findShortestBoundaryExpansionIdx(
   const BoundaryExpansions & boundary_expansions)
 {
diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
index 6c223176c23..2b38d854255 100644
--- a/nav2_smac_planner/test/test_smoother.cpp
+++ b/nav2_smac_planner/test/test_smoother.cpp
@@ -37,11 +37,6 @@ class SmootherWrapper : public nav2_smac_planner::Smoother
   explicit SmootherWrapper(const SmootherParams & params)
   : nav2_smac_planner::Smoother(params)
   {}
-
-  std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
-  {
-    return findDirectionalPathSegments(path);
-  }
 };
 
 TEST(SmootherTest, test_full_smoother)
@@ -133,10 +128,6 @@ TEST(SmootherTest, test_full_smoother)
     y_m = path[i].y;
   }
 
-  // Check that we accurately detect that this path has a reversing segment
-  auto path_segs = smoother->findDirectionalPathSegmentsWrapper(plan);
-  EXPECT_TRUE(path_segs.size() == 2u || path_segs.size() == 3u);
-
   // Test smoother, should succeed with same number of points
   // and shorter overall length, while still being collision free.
   auto path_size_in = plan.poses.size();
diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
index 1f56e8440e5..9a20f19fa59 100644
--- a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
+++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp
@@ -25,7 +25,7 @@
 #include <utility>
 
 #include "nav2_core/smoother.hpp"
-#include "nav2_smoother/smoother_utils.hpp"
+#include "nav2_util/smoother_utils.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"
diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp
index dfaeba953aa..566c5700b76 100644
--- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp
+++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp
@@ -24,7 +24,7 @@
 #include <utility>
 
 #include "nav2_core/smoother.hpp"
-#include "nav2_smoother/smoother_utils.hpp"
+#include "nav2_util/smoother_utils.hpp"
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"
diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp
index 90180734e5c..e38b5134ea4 100644
--- a/nav2_smoother/src/savitzky_golay_smoother.cpp
+++ b/nav2_smoother/src/savitzky_golay_smoother.cpp
@@ -19,11 +19,10 @@
 
 namespace nav2_smoother
 {
-
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
 using nav2::declare_parameter_if_not_declared;
+using nav2_util::PathSegment;
 
 void SavitzkyGolaySmoother::configure(
   const nav2::LifecycleNode::WeakPtr & parent,
@@ -89,7 +88,7 @@ bool SavitzkyGolaySmoother::smooth(
   std::vector<PathSegment> path_segments{
     PathSegment{0u, static_cast<unsigned int>(path.poses.size() - 1)}};
   if (enforce_path_inversion_) {
-    path_segments = findDirectionalPathSegments(path);
+    path_segments = nav2_util::findDirectionalPathSegments(path);
   }
 
   // Minimum point size to smooth is SG filter size + start + end
@@ -171,7 +170,7 @@ bool SavitzkyGolaySmoother::smoothImpl(
     }
   }
 
-  updateApproximatePathOrientations(path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(path, reversing_segment);
   return true;
 }
 
diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp
index 5179784f83a..bb93172f81c 100644
--- a/nav2_smoother/src/simple_smoother.cpp
+++ b/nav2_smoother/src/simple_smoother.cpp
@@ -19,10 +19,10 @@
 
 namespace nav2_smoother
 {
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_util::geometry_utils;  // NOLINT
 using namespace std::chrono;  // NOLINT
 using nav2::declare_parameter_if_not_declared;
+using nav2_util::PathSegment;
 
 void SimpleSmoother::configure(
   const nav2::LifecycleNode::WeakPtr & parent,
@@ -72,10 +72,10 @@ bool SimpleSmoother::smooth(
   nav_msgs::msg::Path curr_path_segment;
   curr_path_segment.header = path.header;
 
-  std::vector<PathSegment> path_segments{PathSegment{
+  std::vector<nav2_util::PathSegment> path_segments{PathSegment{
       0u, static_cast<unsigned int>(path.poses.size() - 1)}};
   if (enforce_path_inversion_) {
-    path_segments = findDirectionalPathSegments(path);
+    path_segments = nav2_util::findDirectionalPathSegments(path);
   }
 
   std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));
@@ -137,7 +137,7 @@ void SimpleSmoother::smoothImpl(
         logger_,
         "Number of iterations has exceeded limit of %i.", max_its_);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment);
       return;
     }
 
@@ -149,7 +149,7 @@ void SimpleSmoother::smoothImpl(
         logger_,
         "Smoothing time exceeded allowed duration of %0.2f.", max_time);
       path = last_path;
-      updateApproximatePathOrientations(path, reversing_segment);
+      nav2_util::updateApproximatePathOrientations(path, reversing_segment);
       throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration");
     }
 
@@ -183,7 +183,7 @@ void SimpleSmoother::smoothImpl(
           "Smoothing process resulted in an infeasible collision. "
           "Returning the last path before the infeasibility was introduced.");
         path = last_path;
-        updateApproximatePathOrientations(path, reversing_segment);
+        nav2_util::updateApproximatePathOrientations(path, reversing_segment);
         return;
       }
     }
@@ -198,7 +198,7 @@ void SimpleSmoother::smoothImpl(
     smoothImpl(new_path, reversing_segment, costmap, max_time);
   }
 
-  updateApproximatePathOrientations(new_path, reversing_segment);
+  nav2_util::updateApproximatePathOrientations(new_path, reversing_segment);
   path = new_path;
 }
 
diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
index b31587a53f7..b1b59c3f2d8 100644
--- a/nav2_smoother/test/test_savitzky_golay_smoother.cpp
+++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp
@@ -30,7 +30,6 @@
 #include "nav2_smoother/savitzky_golay_smoother.hpp"
 #include "ament_index_cpp/get_package_share_directory.hpp"
 
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_smoother;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT
 
diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp
index 311b9838276..be1fadb2338 100644
--- a/nav2_smoother/test/test_simple_smoother.cpp
+++ b/nav2_smoother/test/test_simple_smoother.cpp
@@ -24,7 +24,6 @@
 #include "nav2_smoother/simple_smoother.hpp"
 #include "nav2_core/smoother_exceptions.hpp"
 
-using namespace smoother_utils;  // NOLINT
 using namespace nav2_smoother;  // NOLINT
 using namespace std::chrono_literals;  // NOLINT
 
@@ -36,11 +35,6 @@ class SmootherWrapper : public nav2_smoother::SimpleSmoother
   {
   }
 
-  std::vector<PathSegment> findDirectionalPathSegmentsWrapper(nav_msgs::msg::Path path)
-  {
-    return findDirectionalPathSegments(path);
-  }
-
   void setMaxItsToInvalid()
   {
     max_its_ = 0;
diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt
index 2a2f4061818..f0c51b06905 100644
--- a/nav2_util/CMakeLists.txt
+++ b/nav2_util/CMakeLists.txt
@@ -21,6 +21,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(angles REQUIRED)
 
 nav2_package()
 
@@ -61,6 +62,7 @@ ament_export_dependencies(
   tf2_ros
   pluginlib
   nav2_ros_common
+  angles
 )
 ament_export_targets(${library_name})
 
diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_util/include/nav2_util/smoother_utils.hpp
similarity index 71%
rename from nav2_smoother/include/nav2_smoother/smoother_utils.hpp
rename to nav2_util/include/nav2_util/smoother_utils.hpp
index 500dd7a9351..979345ad7e4 100644
--- a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp
+++ b/nav2_util/include/nav2_util/smoother_utils.hpp
@@ -12,31 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License. Reserved.
 
-#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
-#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
+#ifndef NAV2_UTIL__SMOOTHER_UTILS_HPP_
+#define NAV2_UTIL__SMOOTHER_UTILS_HPP_
 
 #include <cmath>
 #include <vector>
 #include <string>
-#include <iostream>
 #include <memory>
-#include <queue>
 #include <utility>
 
-#include "nav2_core/smoother.hpp"
-#include "nav2_costmap_2d/costmap_2d.hpp"
-#include "nav2_costmap_2d/cost_values.hpp"
 #include "nav2_util/geometry_utils.hpp"
-#include "nav2_ros_common/node_utils.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "angles/angles.h"
 #include "tf2/utils.hpp"
 
-namespace smoother_utils
+namespace nav2_util
 {
 
 /**
- * @class nav2_smoother::PathSegment
+ * @class nav2_util::PathSegment
  * @brief A segment of a path in start/end indices
  */
 struct PathSegment
@@ -45,16 +39,29 @@ struct PathSegment
   unsigned int end;
 };
 
-typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
-typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
-
+/**
+ * @brief Finds the starting and end indices of path segments where
+ * the robot is traveling in the same direction (e.g. forward vs reverse)
+ * @param path Path in which to look for cusps
+ * @param is_holonomic Whether the motion model is holonomic (default is false)
+ Only set as true when the input path is known to be generated from a holonomic planner like NavFn.
+ * @return Set of index pairs for each segment of the path in a given direction
+ */
 inline std::vector<PathSegment> findDirectionalPathSegments(
-  const nav_msgs::msg::Path & path)
+  const nav_msgs::msg::Path & path, bool is_holonomic = false)
 {
   std::vector<PathSegment> segments;
   PathSegment curr_segment;
   curr_segment.start = 0;
 
+  // If holonomic, no directional changes and
+  // may have abrupt angular changes from naive grid search
+  if (is_holonomic) {
+    curr_segment.end = path.poses.size() - 1;
+    segments.push_back(curr_segment);
+    return segments;
+  }
+
   // Iterating through the path to determine the position of the cusp
   for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
     // We have two vectors for the dot product OA and AB. Determining the vectors.
@@ -91,9 +98,17 @@ inline std::vector<PathSegment> findDirectionalPathSegments(
   return segments;
 }
 
+/**
+ * @brief For a given path, update the path point orientations based on smoothing
+ * @param path Path to approximate the path orientation in
+ * @param reversing_segment Return if this is a reversing segment
+ * @param is_holonomic Whether the motion model is holonomic (default is false)
+ Only set as true when the input path is known to be generated from a holonomic planner like NavFn.
+ */
 inline void updateApproximatePathOrientations(
   nav_msgs::msg::Path & path,
-  bool & reversing_segment)
+  bool & reversing_segment,
+  bool is_holonomic = false)
 {
   double dx, dy, theta, pt_yaw;
   reversing_segment = false;
@@ -103,7 +118,7 @@ inline void updateApproximatePathOrientations(
   dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
   theta = atan2(dy, dx);
   pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
-  if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
+  if (!is_holonomic && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
     reversing_segment = true;
   }
 
@@ -127,6 +142,6 @@ inline void updateApproximatePathOrientations(
   }
 }
 
-}  // namespace smoother_utils
+}  // namespace nav2_util
 
-#endif  // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_
+#endif  // NAV2_UTIL__SMOOTHER_UTILS_HPP_
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index 334879c3981..b8c490de287 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/package.xml
@@ -32,6 +32,7 @@
   <depend>tf2_ros</depend>
   <depend>pluginlib</depend>
   <depend>nav2_ros_common</depend>
+  <depend>angles</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index c8447eaab6e..b7f290f8e78 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -21,6 +21,9 @@ target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS})
 ament_add_gtest(test_controller_utils test_controller_utils.cpp)
 target_link_libraries(test_controller_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS})
 
+ament_add_gtest(test_smoother_utils test_smoother_utils.cpp)
+target_link_libraries(test_smoother_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS} angles::angles)
+
 ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp)
 target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>")
 
diff --git a/nav2_util/test/test_smoother_utils.cpp b/nav2_util/test/test_smoother_utils.cpp
new file mode 100644
index 00000000000..b5fdbbff0d9
--- /dev/null
+++ b/nav2_util/test/test_smoother_utils.cpp
@@ -0,0 +1,100 @@
+// Copyright (c) 2025 Maurice Alexander Purnawan
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <cmath>
+#include "nav2_util/smoother_utils.hpp"
+#include "gtest/gtest.h"
+
+using nav2_util::findDirectionalPathSegments;
+using nav2_util::updateApproximatePathOrientations;
+
+geometry_msgs::msg::PoseStamped makePose(double x, double y, double yaw = 0.0)
+{
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = x;
+  pose.pose.position.y = y;
+  pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(yaw);
+  return pose;
+}
+
+TEST(SmootherUtils, HolonomicSingleSegment)
+{
+  nav_msgs::msg::Path path;
+  path.poses.push_back(makePose(0, 0));
+  path.poses.push_back(makePose(1, 0));
+  path.poses.push_back(makePose(2, 0));
+
+  auto segments = findDirectionalPathSegments(path, true);
+  ASSERT_EQ(segments.size(), 1u);
+  EXPECT_EQ(segments[0].start, 0u);
+  EXPECT_EQ(segments[0].end, 2u);
+}
+
+TEST(SmootherUtils, ForwardAndReverseSegments)
+{
+  nav_msgs::msg::Path path;
+  path.poses.push_back(makePose(0, 0));
+  path.poses.push_back(makePose(1, 0));
+  path.poses.push_back(makePose(0, 0));
+  path.poses.push_back(makePose(-1, 0));
+
+  auto segments = findDirectionalPathSegments(path, false);
+  ASSERT_GE(segments.size(), 2u);
+  EXPECT_EQ(segments[0].start, 0u);
+  EXPECT_LT(segments[0].end, path.poses.size());
+}
+
+TEST(SmootherUtils, RotationInPlaceCreatesSegment)
+{
+  nav_msgs::msg::Path path;
+  // Same position, but rotating
+  path.poses.push_back(makePose(0, 0, 0.0));
+  path.poses.push_back(makePose(0, 0, 0.0));
+  path.poses.push_back(makePose(0, 0, M_PI_2));
+  path.poses.push_back(makePose(0, 0, M_PI));
+
+  auto segments = findDirectionalPathSegments(path, false);
+  ASSERT_GE(segments.size(), 2u);
+}
+
+TEST(SmootherUtils, UpdateApproximatePathForward)
+{
+  nav_msgs::msg::Path path;
+  path.poses.push_back(makePose(0, 0, 0.0));
+  path.poses.push_back(makePose(1, 0, 0.0));
+  path.poses.push_back(makePose(2, 0, 0.0));
+
+  bool reversing = false;
+  updateApproximatePathOrientations(path, reversing, false);
+
+  EXPECT_FALSE(reversing);
+  double yaw0 = tf2::getYaw(path.poses[0].pose.orientation);
+  EXPECT_NEAR(yaw0, 0.0, 1e-5);
+}
+
+TEST(SmootherUtils, UpdateApproximatePathReverse)
+{
+  nav_msgs::msg::Path path;
+  path.poses.push_back(makePose(0, 0, 0));
+  path.poses.push_back(makePose(-1, 0, 0));
+  path.poses.push_back(makePose(-2, 0, 0));
+
+  bool reversing = false;
+  updateApproximatePathOrientations(path, reversing, false);
+
+  EXPECT_TRUE(reversing);
+  double yaw0 = tf2::getYaw(path.poses[0].pose.orientation);
+  EXPECT_NEAR(yaw0, 0, 1e-5);
+}