adding checks on goal IDs in results for waypoint follower (#3130)

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

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;