diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp
index fab5d3f0fcf..dd5ac3f3e5f 100644
--- a/nav2_navfn_planner/src/navfn_planner.cpp
+++ b/nav2_navfn_planner/src/navfn_planner.cpp
@@ -353,11 +353,11 @@ NavfnPlanner::smoothApproachToGoal(
   const geometry_msgs::msg::Pose & goal,
   nav_msgs::msg::Path & plan)
 {
-  // Replace the last pose of the computed path if it's actually further away
-  // to the second to last pose than the goal pose.
   if (plan.poses.size() >= 2) {
     auto second_to_last_pose = plan.poses.end()[-2];
     auto last_pose = plan.poses.back();
+    // Replace the last pose of the computed path if it's actually further away
+    // to the second to last pose than the goal pose.
     if (
       squared_distance(last_pose.pose, second_to_last_pose.pose) >
       squared_distance(goal, second_to_last_pose.pose))
@@ -365,6 +365,11 @@ NavfnPlanner::smoothApproachToGoal(
       plan.poses.back().pose = goal;
       return;
     }
+    // Replace the last pose of the computed path if its position matches but orientation differs
+    if (squared_distance(last_pose.pose, goal) < 1e-6) {
+      plan.poses.back().pose = goal;
+      return;
+    }
   }
   geometry_msgs::msg::PoseStamped goal_copy;
   goal_copy.pose = goal;