diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java
index 15148408c80d4601c379c701ec2c83439eab90d6..a2fbed963684ff9871ab376e67ca3ecc6cc37d2d 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java
@@ -314,6 +314,21 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     }
   }
 
+  /**
+   * Set goal ID and state information to the goal status message.
+   * @param gstat GoalStatus message.
+   * @param gidString String identifying the goal.
+   * @see actionlib_msgs.GoalStatus
+   */
+  public void setGoalStatus(GoalStatus gstat, String gidString) {
+    try {
+      ServerGoal serverGoal = goalTracker.get(gidString);
+      gstat.setGoalId(getGoalId(serverGoal.goal));
+      gstat.setStatus((byte)serverGoal.state.getState());
+    }
+    catch (Exception e) {
+    }
+  }
   /**
   * Publishes the server's topics and suscribes to the client's topics.
   */