From 5d59acbec66b33b3437e8968c766b55a98e48287 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 28 Aug 2025 02:47:03 +0800 Subject: [PATCH] Replace last pose if only orientation differs in Navfn (#5490) Signed-off-by: mini-1235 (cherry picked from commit ff80727eee2bc5f3d70a849d9e821bc01eee757c) --- nav2_navfn_planner/src/navfn_planner.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index e42c26202a5..06ff387d267 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;