From b8d8176d05a27a49a94cf18c7e0fa37020be4f0c Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sat, 17 Aug 2024 23:15:37 +0200 Subject: [PATCH 1/3] 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 403c007fb0..ca7a67d642 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; } From a8d4cbc11ca8d2bca1e1bdb27bca51d27b9e1161 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Wed, 11 Sep 2024 18:50:34 +0200 Subject: [PATCH 2/3] removing the short circuit Signed-off-by: Pradheep --- .../plugins/decorator/path_longer_on_approach.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ca7a67d642..1b7ac5c33f 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -35,7 +35,7 @@ bool PathLongerOnApproach::isPathUpdated( 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() && + new_path.poses.size() != 0 && old_path.poses.back().pose.position == new_path.poses.back().pose.position; } From 240750f6293a5ea04147c6c071e91cbb5e5c83f6 Mon Sep 17 00:00:00 2001 From: Pradheep Date: Sun, 15 Sep 2024 20:38:54 +0200 Subject: [PATCH 3/3] adding additional layer of check Signed-off-by: Pradheep --- nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp | 1 + 1 file changed, 1 insertion(+) 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 1b7ac5c33f..443a29acf0 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,6 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { 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; }