From b8c90622ba8ce0799a6f2510e3476067ec5f86db Mon Sep 17 00:00:00 2001
From: Ernesto Corbellini <ecorbellini@ekumenlabs.com>
Date: Mon, 14 Dec 2015 15:18:35 -0300
Subject: [PATCH] Added callback for feedback message.

---
 .../rosjava_actionlib/ActionClient.java       | 16 ++++++++--
 .../ActionClientListener.java                 |  4 ++-
 .../ekumen/rosjava_actionlib/TestClient.java  | 32 ++++++++++++++-----
 3 files changed, 40 insertions(+), 12 deletions(-)

diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
index db58614..2ee6fa5 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
@@ -9,6 +9,7 @@ import org.ros.node.topic.Publisher;
 import org.ros.message.MessageListener;
 import org.ros.internal.message.Message;
 import java.util.concurrent.TimeUnit;
+import actionlib_msgs.GoalStatusArray;
 
 public class ActionClient<T_ACTION_GOAL extends Message,
   T_ACTION_FEEDBACK extends Message,
@@ -23,6 +24,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
   //Suscriber<actionlib_msgs.status> serverStatus;
   Subscriber<T_ACTION_RESULT> serverResult = null;
   Subscriber<T_ACTION_FEEDBACK> serverFeedback = null;
+  Subscriber<GoalStatusArray> serverStatus = null;
   ConnectedNode node = null;
   String actionName;
   ActionClientListener callbackTarget = null;
@@ -65,6 +67,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
   private void subscribeToServer(ConnectedNode node) {
     serverResult = node.newSubscriber(actionName + "/result", actionResultType);
     serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType);
+    serverStatus = node.newSubscriber(actionName + "/status", GoalStatusArray._TYPE);
 
     serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() {
       @Override
@@ -80,10 +83,10 @@ public class ActionClient<T_ACTION_GOAL extends Message,
       }
     });
 
-    serverResult.addMessageListener(new MessageListener<T_ACTION_RESULT>() {
+    serverStatus.addMessageListener(new MessageListener<GoalStatusArray>() {
       @Override
-      public void onNewMessage(T_ACTION_RESULT message) {
-        gotResult(message);
+      public void onNewMessage(GoalStatusArray message) {
+        gotStatus(message);
       }
     });
 
@@ -116,6 +119,13 @@ public class ActionClient<T_ACTION_GOAL extends Message,
     }
   }
 
+  public void gotStatus(GoalStatusArray message) {
+    // Propagate the callback
+    if (callbackTarget != null) {
+      callbackTarget.statusReceived(message);
+    }
+  }
+
   /**
   * Publishes the client's topics and suscribes to the server's topics.
   */
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClientListener.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClientListener.java
index 680c731..fec5aca 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClientListener.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClientListener.java
@@ -1,6 +1,8 @@
 package com.github.ekumen.rosjava_actionlib;
 
 import org.ros.internal.message.Message;
+import actionlib_msgs.GoalStatusArray;
+
 
 /**
  * Listener interface to receive the incoming messages from the ActionLib server.
@@ -10,5 +12,5 @@ import org.ros.internal.message.Message;
 public interface ActionClientListener<T_ACTION_FEEDBACK extends Message, T_ACTION_RESULT extends Message> {
   void resultReceived(T_ACTION_RESULT result);
   void feedbackReceived(T_ACTION_FEEDBACK feedback);
-  void statusReceived(Message status);
+  void statusReceived(GoalStatusArray status);
 }
diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
index 9851afa..08db54b 100644
--- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
+++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
@@ -10,9 +10,11 @@ import actionlib_tutorials.FibonacciActionResult;
 import actionlib_tutorials.FibonacciGoal;
 import actionlib_tutorials.FibonacciFeedback;
 import actionlib_tutorials.FibonacciResult;
+import actionlib_msgs.GoalStatusArray;
 
 public class TestClient extends AbstractNodeMain implements ActionClientListener<FibonacciActionFeedback, FibonacciActionResult> {
   ActionClient ac;
+  volatile private boolean resultReceived = false;
 
   @Override
   public GraphName getDefaultNodeName() {
@@ -22,6 +24,7 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
   @Override
   public void onStart(ConnectedNode node) {
     ac = new ActionClient<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE, FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE);
+    int repeat = 3;
 
     // Attach listener for the callbacks
     ac.attachListener(this);
@@ -34,15 +37,17 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
     fibonacciGoal.setOrder(6);
     goalMessage.setGoal(fibonacciGoal);
 
-    while (true) {
+    while (repeat > 0) {
+      System.out.println("Sending goal #" + repeat + "...");
       ac.sendGoal(goalMessage);
-      try {
-        Thread.sleep(10000);
-      }
-      catch (InterruptedException ex) {
-        ;
-      }
+      System.out.println("Goal sent.");
+      sleep(10000);
+      //while(!resultReceived) sleep(100);
+      resultReceived = false;
+      repeat--;
     }
+
+    System.exit(0);
   }
 
   @Override
@@ -51,6 +56,8 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
     int[] sequence = result.getSequence();
     int i;
 
+    resultReceived = true;
+
     System.out.print("Got Fibonacci result sequence!");
     for (i=0; i<sequence.length; i++)
       System.out.print(Integer.toString(sequence[i]) + " ");
@@ -70,7 +77,16 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
   }
 
   @Override
-  public void statusReceived(Message status) {
+  public void statusReceived(GoalStatusArray status) {
+
+  }
 
+  void sleep(long msec) {
+    try {
+      Thread.sleep(msec);
+    }
+    catch (InterruptedException ex) {
+      ;
+    }
   }
 }
-- 
GitLab