Added goal accept mechanism and succeed for the server.

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

@@ -11,9 +11,11 @@ import java.util.concurrent.TimeUnit;
 import java.util.Timer;
 import java.util.TimerTask;
 import java.util.HashMap;
+import java.util.Vector;
 import java.lang.reflect.Method;
 import actionlib_msgs.GoalStatusArray;
 import actionlib_msgs.GoalID;
+import actionlib_msgs.GoalStatus;
 
 public class ActionServer<T_ACTION_GOAL extends Message,
   T_ACTION_FEEDBACK extends Message,
@@ -99,6 +101,10 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     }
   }
 
+  /**
+   * Subscribe to the action client's topics: goal and cancel.
+   * @param node The ROS node connected to the master.
+   */
   private void subscribeToClient(ConnectedNode node) {
     goalSuscriber = node.newSubscriber(actionName + "/goal", actionGoalType);
     cancelSuscriber = node.newSubscriber(actionName + "/cancel", GoalID._TYPE);
@@ -118,6 +124,9 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     });
   }
 
+  /**
+   * Unsubscribe from the client's topics.
+   */
   private void unsubscribeToClient() {
     if (goalSuscriber != null) {
       goalSuscriber.shutdown(5, TimeUnit.SECONDS);
@@ -129,14 +138,44 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     }
   }
 
+  /**
+   * Called when we get a message on the subscribed goal topic.
+   */
   public void gotGoal(T_ACTION_GOAL goal) {
-    goalTracker.put(getGoalId(goal).getId(), new ServerGoal(goal));
+    boolean accepted =  false;
+    String goalIdString = getGoalId(goal).getId();
+
+    // start tracking this newly received goal
+    goalTracker.put(goalIdString, new ServerGoal(goal));
     // Propagate the callback
     if (callbackTarget != null) {
+      // inform the user of a received message
       callbackTarget.goalReceived(goal);
+      // ask the user to accept the goal
+      accepted = callbackTarget.acceptGoal(goal);
+      if (accepted) {
+        // the user accepted the goal
+        try {
+          goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.ACCEPT);
+        }
+        catch (Exception e) {
+          e.printStackTrace(System.out);
+        }
+      } else {
+        // the user rejected the goal
+        try {
+          goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.REJECT);
+        }
+        catch (Exception e) {
+          e.printStackTrace(System.out);
+        }
+      }
     }
   }
 
+  /**
+   * Called when we get a message on the subscribed cancel topic.
+   */
   public void gotCancel(GoalID gid) {
     // Propagate the callback
     if (callbackTarget != null) {
@@ -144,8 +183,22 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     }
   }
 
+  /**
+   * Publishes the current status on the server's status topic.
+   * This is used like a heartbeat to update the status of every tracked goal.
+   */
   public void sendStatusTick() {
     GoalStatusArray status = statusPublisher.newMessage();
+    GoalStatus goalStatus;
+    Vector<GoalStatus> goalStatusList = new Vector<GoalStatus>();
+
+    for (ServerGoal sg : goalTracker.values()) {
+      goalStatus = node.getTopicMessageFactory().newFromType(GoalStatus._TYPE);
+      goalStatus.setGoalId(getGoalId(sg.goal));
+      goalStatus.setStatus((byte)sg.state.getState());
+      goalStatusList.add(goalStatus);
+    }
+    status.setStatusList(goalStatusList);
     sendStatus(status);
   }
 
@@ -157,6 +210,11 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     return feedbackPublisher.newMessage();
   }
 
+  /**
+   * Returns the goal ID object related to a given action goal.
+   * @param goal An action goal message.
+   * @return The goal ID object.
+   */
   public GoalID getGoalId(T_ACTION_GOAL goal) {
     GoalID gid = null;
     try {
@@ -170,6 +228,34 @@ public class ActionServer<T_ACTION_GOAL extends Message,
     return gid;
   }
 
+  /**
+   * Get the current state of the referenced goal.
+   * @param goalId String representing the ID of the goal.
+   * @return The current state of the goal or -100 if the goal ID is not tracked.
+   * @see actionlib_msgs.GoalStatus
+   */
+  public int getGoalState(String goalId) {
+    int ret = 0;
+
+    if (goalTracker.containsKey(goalId)) {
+      ret = goalTracker.get(goalId).state.getState();
+    } else {
+      ret = -100;
+    }
+    return ret;
+  }
+
+  /**
+   * Express a succeed event for this goal. The state of the goal will be updated.
+   */
+  public void setSucceed(String goalIdString) {
+    try {
+      goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.SUCCEED);
+    }
+    catch (Exception e) {
+    }
+  }
+
   /**
   * Publishes the server's topics and suscribes to the client's topics.
   */

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

@@ -10,6 +10,25 @@ import actionlib_msgs.GoalID;
  * with information from the client.
  */
 public interface ActionServerListener<T_ACTION_GOAL extends Message> {
+  /**
+   * This callback is called when a message is received on the goal topic.
+   * Note: this method is called right after the server starts tracking this
+   * goal and is intended for informative purposes.
+   * @param goal the action goal received.
+   */
   void goalReceived(T_ACTION_GOAL goal);
+
+  /**
+   * This callback is called when a message is received on the cancel topic.
+   * @param id Goal ID object that was received.
+   */
   void cancelReceived(GoalID id);
+
+  /**
+   * Callback method to accept a recently received action goal.
+   * @param goal The action goal received.
+   * @return The implementer must return true if he accepts the goal or false
+   * otherwise.
+   */
+  boolean acceptGoal(T_ACTION_GOAL goal);
 }

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

@@ -17,6 +17,7 @@ import actionlib_msgs.GoalStatus;
 
 public class TestServer extends AbstractNodeMain implements ActionServerListener<FibonacciActionGoal> {
   private ActionServer<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult> as = null;
+  private volatile FibonacciActionGoal currentGoal = null;
 
   @Override
   public GraphName getDefaultNodeName() {
@@ -25,21 +26,74 @@ public class TestServer extends AbstractNodeMain implements ActionServerListener
 
   @Override
   public void onStart(ConnectedNode node) {
+    FibonacciActionResult result;
+
     as = new ActionServer<FibonacciActionGoal, FibonacciActionFeedback,
       FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE,
       FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE);
 
     as.attachListener(this);
+
+    while(true) {
+      if (currentGoal != null) {
+        result = as.newResultMessage();
+        result.getResult().setSequence(fibonacciSequence(currentGoal.getGoal().getOrder()));
+        as.setSucceed(currentGoal.getGoalId().getId());
+        as.sendResult(result);
+        currentGoal = null;
+      }
+    }
   }
 
   @Override
   public void goalReceived(FibonacciActionGoal goal) {
     System.out.println("Goal received.");
-    as.sendResult(as.newResultMessage());
+    sleep(2000);
+    System.out.println("Sending result...");
+    sleep(2000);as.sendResult(as.newResultMessage());
   }
 
   @Override
   public void cancelReceived(GoalID id) {
     System.out.println("Cancel received.");
   }
+
+  @Override
+  public boolean acceptGoal(FibonacciActionGoal goal) {
+    // If we don't have a goal, accept it. Otherwise, reject it.
+    if (currentGoal == null) {
+      currentGoal = goal;
+      System.out.println("Goal accepted.");
+      return true;
+    } else {
+      System.out.println("We already have a goal! New goal reject.");
+      return false;
+    }
+  }
+
+  private int[] fibonacciSequence(int order) {
+    int i;
+    int[] fib = new int[order + 2];
+
+    fib[0] = 0;
+    fib[1] = 1;
+
+    for (i = 2; i < (order + 2); i++) {
+      fib[i] = fib[i - 1] + fib[i - 2];
+    }
+    return fib;
+  }
+
+  /*
+   * Sleep for an amount on miliseconds.
+   * @param msec Number or miliseconds to sleep.
+   */
+  private void sleep(long msec) {
+    try {
+      Thread.sleep(msec);
+    }
+    catch (InterruptedException ex) {
+    }
+  }
+
 }