Added result received event status update.

Author: Ernesto Corbellini

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

@@ -162,6 +162,7 @@ public void setGoalId(T_ACTION_GOAL goal, GoalID gid) {
    * @see actionlib_msgs.GoalID
    */
   public void sendCancel(GoalID id) {
+    goalManager.cancelGoal();
     cancelPublisher.publish(id);
   }
 
@@ -251,6 +252,11 @@ private void unsubscribeToServer() {
    * depends on the application.
    */
   public void gotResult(T_ACTION_RESULT message) {
+    ActionResult<T_ACTION_RESULT> ar = new ActionResult(message);
+    if (ar.getGoalStatusMessage().getGoalId().getId().equals(goalManager.actionGoal.getGoalId())) {
+      goalManager.updateStatus(ar.getGoalStatusMessage().getStatus());
+    }
+    goalManager.resultReceived();
     // Propagate the callback
     if (callbackTarget != null) {
       callbackTarget.resultReceived(message);

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

@@ -0,0 +1,81 @@
+/**
+ * Copyright 2015 Ekumen www.ekumenlabs.com
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+package com.github.ekumen.rosjava_actionlib;
+
+import java.lang.reflect.Method;
+import org.ros.internal.message.Message;
+import org.ros.message.Time;
+import std_msgs.Header;
+import actionlib_msgs.GoalID;
+import actionlib_msgs.GoalStatus;
+
+/**
+ * Class to encapsulate the action feedback object.
+ * @author Ernesto Corbellini [email protected]
+ */
+public class ActionResult<T_ACTION_RESULT extends Message> {
+  private T_ACTION_RESULT actionResultMessage = null;
+
+  public ActionResult(T_ACTION_RESULT msg) {
+    actionResultMessage = msg;
+  }
+
+  public Header getHeaderMessage() {
+    Header h = null;
+    if (actionResultMessage != null) {
+      try {
+        Method m = actionResultMessage.getClass().getMethod("getHeader");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        h = (Header)m.invoke(actionResultMessage);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return h;
+  }
+
+  public GoalStatus getGoalStatusMessage() {
+    GoalStatus gs = null;
+    if (actionResultMessage != null) {
+      try {
+        Method m = actionResultMessage.getClass().getMethod("getStatus");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        gs = (GoalStatus)m.invoke(actionResultMessage);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return gs;
+  }
+
+  public Message getResultMessage() {
+    Message x = null;
+    if (actionResultMessage != null) {
+      try {
+        Method m = actionResultMessage.getClass().getMethod("getResult");
+        m.setAccessible(true); // workaround for known bug http://bugs.java.com/bugdatabase/view_bug.do?bug_id=6924232
+        x = (Message)m.invoke(actionResultMessage);
+      }
+      catch (Exception e) {
+        e.printStackTrace(System.out);
+      }
+    }
+    return x;
+  }
+}

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

@@ -48,6 +48,10 @@ public boolean cancelGoal() {
     return stateMachine.cancel();
   }
 
+  public void resultReceived() {
+    stateMachine.resultReceived();
+  }
+
   public void updateStatus(int status) {
     stateMachine.transition(status);
   }

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

@@ -458,13 +458,19 @@ public boolean cancel() {
       case ClientStates.WAITING_FOR_GOAL_ACK:
       case ClientStates.PENDING:
       case ClientStates.ACTIVE:
-        this.state = ClientStates.WAITING_FOR_CANCEL_ACK;
+        state = ClientStates.WAITING_FOR_CANCEL_ACK;
         ret = true;
         break;
     }
     return ret;
   }
 
+  public void resultReceived() {
+    if (state == ClientStates.WAITING_FOR_RESULT) {
+      state = ClientStates.DONE;
+    }
+  }
+
   public void markAsLost()
   {
   }

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

@@ -83,15 +83,16 @@ public void onStart(ConnectedNode node) {
     // send another message and cancel it
     goalId += i;
     goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
-    goalMessage.getGoal().setOrder(2);
-    System.out.println("Sending goal ID: " + goalId + "...");
-    ac.sendGoal(goalMessage, goalId);
-    System.out.println("Goal sent.");
-    sleep(2000);
-    System.out.println("Cancelling goal ID: " + goalId);
+    goalMessage.getGoal().setOrder(3);
+    //System.out.println("Sending goal ID: " + goalId + "...");
+    //ac.sendGoal(goalMessage, goalId);
+    ac.sendGoal(goalMessage);
     GoalID gid = ac.getGoalId(goalMessage);
-    ac.sendCancel(gid);
-    sleep(10000);
+    System.out.println("Goal sent. Goal ID: " + gid);
+    //sleep(1000);
+    //System.out.println("Cancelling goal ID: " + goalId);
+    //ac.sendCancel(gid);
+    sleep(5000);
 
     System.exit(0);
   }