From 187a3de3b5579343f84311b394b6be71549377a1 Mon Sep 17 00:00:00 2001 From: Ernesto Corbellini <ecorbellini@ekumenlabs.com> Date: Wed, 13 Jan 2016 18:02:17 -0300 Subject: [PATCH] Added method to wait for the server. --- .../rosjava_actionlib/ActionClient.java | 45 ++++++++++++++++++- .../ekumen/rosjava_actionlib/TestClient.java | 8 +++- 2 files changed, 50 insertions(+), 3 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 5b90aa8..b585b73 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 @@ -21,12 +21,17 @@ import org.ros.node.AbstractNodeMain; 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 @@ import actionlib_msgs.GoalID; */ 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 @@ public class ActionClient<T_ACTION_GOAL extends Message, 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 class ActionClient<T_ACTION_GOAL extends 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 @@ public class ActionClient<T_ACTION_GOAL extends Message, 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/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 b77ed32..f58714f 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 @@ -55,6 +55,10 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener // 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 class TestClient extends AbstractNodeMain implements ActionClientListener //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; -- GitLab