ActionClient: updated goal management.

Added events for sendgoal and statusreceived.
ClientGoalManager: enforce construction with action goal; goal setter and status updater.

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

@@ -27,8 +27,10 @@ import org.ros.node.topic.DefaultSubscriberListener;
 import org.ros.message.MessageListener;
 import org.ros.internal.message.Message;
 import java.util.concurrent.TimeUnit;
+import java.util.List;
 import java.lang.reflect.Method;
 import actionlib_msgs.GoalStatusArray;
+import actionlib_msgs.GoalStatus;
 import actionlib_msgs.GoalID;
 import org.apache.commons.logging.Log;
 import org.apache.commons.logging.LogFactory;
@@ -43,7 +45,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
   T_ACTION_RESULT extends Message> extends DefaultSubscriberListener {
 
   T_ACTION_GOAL actionGoal;
-  ClientGoalManager goalManager;
+  ClientGoalManager<T_ACTION_GOAL> goalManager;
   String actionGoalType;
   String actionResultType;
   String actionFeedbackType;
@@ -83,7 +85,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     this.actionResultType = actionResultType;
     goalIdGenerator = new GoalIDGenerator(node);
     connect(node);
-    goalManager = new ClientGoalManager();
+    goalManager = new ClientGoalManager(new ActionGoal<T_ACTION_GOAL>());
   }
 
   public void attachListener(ActionClientListener target) {
@@ -97,14 +99,15 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     * @param id A string containing the ID for the goal. The ID should represent
     * this goal in a unique fashion in the server and the client.
     */
-  public void sendGoal(T_ACTION_GOAL goal, String id) {
-    GoalID gid = getGoalId(goal);
+  public void sendGoal(T_ACTION_GOAL agMessage, String id) {
+    GoalID gid = getGoalId(agMessage);
     if (id == "") {
       goalIdGenerator.generateID(gid);
     } else {
       gid.setId(id);
     }
-    goalPublisher.publish(goal);
+    goalManager.setGoal(agMessage);
+    goalPublisher.publish(agMessage);
   }
 
   /**
@@ -112,8 +115,8 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     * is dependent on the application. A goal ID will be automatically generated.
     * @param goal The action goal message.
     */
-  public void sendGoal(T_ACTION_GOAL goal) {
-    sendGoal(goal, "");
+  public void sendGoal(T_ACTION_GOAL agMessage) {
+    sendGoal(agMessage, "");
   }
 
   /**
@@ -273,12 +276,38 @@ public class ActionClient<T_ACTION_GOAL extends Message,
    */
   public void gotStatus(GoalStatusArray message) {
     statusReceivedFlag = true;
+    // Find the status for our current goal
+    GoalStatus gstat = findStatus(message);
+    if (gstat != null) {
+      // update the goal status tracking
+      goalManager.updateStatus(gstat.getStatus());
+    }
     // Propagate the callback
     if (callbackTarget != null) {
       callbackTarget.statusReceived(message);
     }
   }
 
+  /**
+   * Walk through the status array and find the status for the action goal that
+   * we are interested in.
+   * @param statusMessage The message with the goal status array
+   * (actionlib_msgs.GoalStatusArray)
+   * @return The goal status message for the goal we want or null if we didn't
+   * find it.
+   */
+  public GoalStatus findStatus(GoalStatusArray statusMessage) {
+    GoalStatus gstat = null;
+    List<GoalStatus> statusList = statusMessage.getStatusList();
+    for (GoalStatus s : statusList) {
+      if (s.getGoalId().getId() == goalManager.actionGoal.getGoalId()) {
+        // this is the goal we are interested in
+        gstat = s;
+      }
+    }
+    return gstat;
+  }
+
   /**
   * Publishes the client's topics and suscribes to the server's topics.
   * @param node The node object that is connected to the ROS master.

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

@@ -24,17 +24,30 @@ import actionlib_msgs.GoalStatus;
  * Class that binds and action goal with a state machine to track its state.
  * @author Ernesto Corbellini ecorbellini@ekumenlabs.com
  */
-public class ClientGoalManager<T_ACTION_GOAL extends Message, T_GOAL extends Message> {
-  public ActionGoal<T_ACTION_GOAL, T_GOAL> actionGoal = null;
+public class ClientGoalManager<T_ACTION_GOAL extends Message> {
+  public ActionGoal<T_ACTION_GOAL> actionGoal = null;
   public ClientStateMachine stateMachine = null;
 
-  public void sendGoal(ActionGoal<T_ACTION_GOAL, T_GOAL> ag) {
+  public ClientGoalManager(ActionGoal<T_ACTION_GOAL> ag) {
+    actionGoal = ag;
+  }
+
+  public void setGoal(ActionGoal<T_ACTION_GOAL> ag) {
     actionGoal = ag;
     stateMachine = new ClientStateMachine();
     stateMachine.setState(ClientStateMachine.ClientStates.WAITING_FOR_GOAL_ACK);
   }
 
+  public void setGoal(T_ACTION_GOAL agm) {
+    ActionGoal<T_ACTION_GOAL> ag = new ActionGoal();
+    ag.setGoalMessage(agm);
+  }
+
   public boolean cancelGoal() {
     return stateMachine.cancel();
   }
+
+  public void updateStatus(int status) {
+    stateMachine.transition(status);
+  }
 }