From 1d745ec7e7821f5e2ed066d7b70935c2ecf12016 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sat, 17 Aug 2024 23:15:37 +0200 Subject: [PATCH] fixing path longer on approach Signed-off-by: Pradheep --- .../plugins/decorator/path_longer_on_approach.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 403c007fb05..ca7a67d6421 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -34,9 +34,9 @@ bool PathLongerOnApproach::isPathUpdated( nav_msgs::msg::Path & new_path, nav_msgs::msg::Path & old_path) { - return new_path != old_path && old_path.poses.size() != 0 && - new_path.poses.size() != 0 && - old_path.poses.back().pose == new_path.poses.back().pose; + 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( @@ -64,7 +64,7 @@ inline BT::NodeStatus PathLongerOnApproach::tick() if (first_time_ == false) { if (old_path_.poses.empty() || new_path_.poses.empty() || - old_path_.poses.back() != new_path_.poses.back()) + old_path_.poses.back().pose != new_path_.poses.back().pose) { first_time_ = true; }