Added method to wait for the server.

fmessmer · Jan 13, 2016 · 187a3de · 187a3de
1 parent db338ef
commit 187a3de
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 3 deletions.
diff --git a/...lib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java b/...lib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionClient.java
@@ -21,12 +21,17 @@
 import org.ros.node.ConnectedNode;
 import org.ros.node.topic.Subscriber;
 import org.ros.node.topic.Publisher;
+import org.ros.node.topic.SubscriberListener;
+import org.ros.internal.node.topic.PublisherIdentifier;
+import org.ros.node.topic.DefaultSubscriberListener;
 import org.ros.message.MessageListener;
 import org.ros.internal.message.Message;
 import java.util.concurrent.TimeUnit;
 import java.lang.reflect.Method;
 import actionlib_msgs.GoalStatusArray;
 import actionlib_msgs.GoalID;
+import org.apache.commons.logging.Log;
+import org.apache.commons.logging.LogFactory;
 
 /**
  * Client implementation for actionlib.
@@ -35,7 +40,7 @@
  */
 public class ActionClient<T_ACTION_GOAL extends Message,
   T_ACTION_FEEDBACK extends Message,
-  T_ACTION_RESULT extends Message> {
+  T_ACTION_RESULT extends Message> extends DefaultSubscriberListener {
 
   T_ACTION_GOAL actionGoal;
   String actionGoalType;
@@ -50,6 +55,10 @@ public class ActionClient<T_ACTION_GOAL extends Message,
   String actionName;
   ActionClientListener callbackTarget = null;
   GoalIDGenerator goalIdGenerator = null;
+  volatile boolean statusReceivedFlag = false;
+  volatile boolean feedbackPublisherFlag = false;
+  volatile boolean resultPublisherFlag = false;
+  private Log log = LogFactory.getLog(ActionClient.class);
 
   /**
    * Constructor for an ActionClient object.
@@ -188,6 +197,9 @@ private void subscribeToServer(ConnectedNode node) {
     serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType);
     serverStatus = node.newSubscriber(actionName + "/status", GoalStatusArray._TYPE);
 
+    serverFeedback.addSubscriberListener(this);
+    serverResult.addSubscriberListener(this);
+
     serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() {
       @Override
       public void onNewMessage(T_ACTION_FEEDBACK message) {
@@ -258,6 +270,7 @@ public void gotFeedback(T_ACTION_FEEDBACK message) {
    * @see actionlib_msgs.GoalStatusArray
    */
   public void gotStatus(GoalStatusArray message) {
+    statusReceivedFlag = true;
     // Propagate the callback
     if (callbackTarget != null) {
       callbackTarget.statusReceived(message);
@@ -273,6 +286,36 @@ private void connect(ConnectedNode node) {
     subscribeToServer(node);
   }
 
+  /**
+   * Wait for an actionlib server to connect.
+   */
+  public boolean waitForActionServerToStart() {
+    boolean res = false;
+
+    while (!res) {
+      res = goalPublisher.hasSubscribers() &&
+        cancelPublisher.hasSubscribers() &&
+        feedbackPublisherFlag &&
+        resultPublisherFlag &&
+        statusReceivedFlag;
+    }
+    return res;
+  }
+
+  @Override
+  public void onNewPublisher(Subscriber subscriber, PublisherIdentifier publisherIdentifier) {
+  //public void onNewFeedbackPublisher(Subscriber<T_ACTION_FEEDBACK> subscriber, PublisherIdentifier publisherIdentifier) {
+    if (subscriber.equals(serverFeedback)) {
+      feedbackPublisherFlag = true;
+      log.info("Found server publishing on the " + actionName + "/feedback topic.");
+    } else {
+      if (subscriber.equals(serverResult)) {
+        resultPublisherFlag = true;
+        log.info("Found server publishing on the " + actionName + "/result topic.");
+      }
+    }
+  }
+
   /**
    * Finish the action client. Unregister publishers and listeners.
    */

diff --git a/...onlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java b/...onlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestClient.java
@@ -55,6 +55,10 @@ public void onStart(ConnectedNode node) {
     // Attach listener for the callbacks
     ac.attachListener(this);
 
+    System.out.println("Waiting for actionlib server to start...");
+    ac.waitForActionServerToStart();
+    System.out.println("actionlib server started.");
+
     // Create Fibonacci goal message
     //goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
     //FibonacciGoal fibonacciGoal = goalMessage.getGoal();
@@ -63,10 +67,10 @@ public void onStart(ConnectedNode node) {
     //fibonacciGoal.setOrder(6);
 
     for (i = 0; i < repeat; i++) {
-      sleep(10000);
+      //sleep(10000);
       System.out.println("Sending goal #" + i + "...");
       goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
-      goalMessage.getGoal().setOrder(i);
+      goalMessage.getGoal().setOrder(i*3);
       ac.sendGoal(goalMessage, goalId + i);
       System.out.println("Goal sent.");
       resultReceived = false;