From 8813663a12fe605e91d38d96df406231410d0759 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Fri, 19 Aug 2022 15:53:18 -0700
Subject: [PATCH] adding checks on goal IDs in results for waypoint follower
 (#3130)

---
 nav2_util/include/nav2_util/simple_action_server.hpp | 12 ++++++++++++
 nav2_waypoint_follower/src/waypoint_follower.cpp     |  7 +++++++
 2 files changed, 19 insertions(+)

diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp
index 40f1a93860..f05a4602ac 100644
--- a/nav2_util/include/nav2_util/simple_action_server.hpp
+++ b/nav2_util/include/nav2_util/simple_action_server.hpp
@@ -390,6 +390,18 @@ class SimpleActionServer
     return current_handle_->get_goal();
   }
 
+  const rclcpp_action::GoalUUID get_current_goal_id() const
+  {
+    std::lock_guard<std::recursive_mutex> lock(update_mutex_);
+
+    if (!is_active(current_handle_)) {
+      error_msg("A goal is not available or has reached a final state");
+      return rclcpp_action::GoalUUID();
+    }
+
+    return current_handle_->get_goal_id();
+  }
+
   /**
    * @brief Get the pending goal object
    * @return Goal Ptr to the goal that's pending
diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp
index 804de9ad31..5de0a1a3f1 100644
--- a/nav2_waypoint_follower/src/waypoint_follower.cpp
+++ b/nav2_waypoint_follower/src/waypoint_follower.cpp
@@ -286,6 +286,13 @@ void
 WaypointFollower::resultCallback(
   const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result)
 {
+  if (result.goal_id != future_goal_handle_.get()->get_goal_id()) {
+    RCLCPP_DEBUG(
+      get_logger(),
+      "Goal IDs do not match for the current goal handle and received result."
+      "Ignoring likely due to receiving result for an old goal.");
+  }
+
   switch (result.code) {
     case rclcpp_action::ResultCode::SUCCEEDED:
       current_goal_status_ = ActionStatus::SUCCEEDED;