Added goal tracker to the server.

src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java

@@ -10,6 +10,8 @@ import org.ros.internal.message.Message;
 import java.util.concurrent.TimeUnit;
 import java.util.Timer;
 import java.util.TimerTask;
+import java.util.HashMap;
+import java.lang.reflect.Method;
 import actionlib_msgs.GoalStatusArray;
 import actionlib_msgs.GoalID;
 
@@ -17,6 +19,15 @@ public class ActionServer<T_ACTION_GOAL extends Message,
   T_ACTION_FEEDBACK extends Message,
   T_ACTION_RESULT extends Message> {
 
+  private class ServerGoal {
+    T_ACTION_GOAL goal;
+    ServerStateMachine state = new ServerStateMachine();
+
+    ServerGoal(T_ACTION_GOAL g) {
+      goal = g;
+    }
+  }
+
   private T_ACTION_GOAL actionGoal;
   private String actionGoalType;
   private String actionResultType;
@@ -31,6 +42,8 @@ public class ActionServer<T_ACTION_GOAL extends Message,
   private String actionName;
   private ActionServerListener callbackTarget = null;
   private Timer statusTick = new Timer();
+  private HashMap<String, ServerGoal> goalTracker = new  HashMap<String,
+    ServerGoal>(1);
 
   ActionServer (ConnectedNode node, String actionName, String actionGoalType,
     String actionFeedbackType, String actionResultType) {
@@ -116,17 +129,18 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     }
   }
 
-  public void gotGoal(T_ACTION_GOAL message) {
+  public void gotGoal(T_ACTION_GOAL goal) {
+    goalTracker.put(getGoalId(goal).getId(), new ServerGoal(goal));
     // Propagate the callback
     if (callbackTarget != null) {
-      callbackTarget.goalReceived(message);
+      callbackTarget.goalReceived(goal);
     }
   }
 
-  public void gotCancel(GoalID message) {
+  public void gotCancel(GoalID gid) {
     // Propagate the callback
     if (callbackTarget != null) {
-      callbackTarget.cancelReceived(message);
+      callbackTarget.cancelReceived(gid);
     }
   }
 
@@ -143,6 +157,19 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     return feedbackPublisher.newMessage();
   }
 
+  public GoalID getGoalId(T_ACTION_GOAL goal) {
+    GoalID gid = null;
+    try {
+      Method m = goal.getClass().getMethod("getGoalId");
+      m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+      gid = (GoalID)m.invoke(goal);
+    }
+    catch (Exception e) {
+      e.printStackTrace(System.out);
+    }
+    return gid;
+  }
+
   /**
   * Publishes the server's topics and suscribes to the client's topics.
   */