Skip to content
Snippets Groups Projects
Commit 187a3de3 authored by Ernesto Corbellini's avatar Ernesto Corbellini
Browse files

Added method to wait for the server.

parent db338ef7
No related branches found
No related tags found
No related merge requests found
...@@ -21,12 +21,17 @@ import org.ros.node.AbstractNodeMain; ...@@ -21,12 +21,17 @@ import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode; import org.ros.node.ConnectedNode;
import org.ros.node.topic.Subscriber; import org.ros.node.topic.Subscriber;
import org.ros.node.topic.Publisher; 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.message.MessageListener;
import org.ros.internal.message.Message; import org.ros.internal.message.Message;
import java.util.concurrent.TimeUnit; import java.util.concurrent.TimeUnit;
import java.lang.reflect.Method; import java.lang.reflect.Method;
import actionlib_msgs.GoalStatusArray; import actionlib_msgs.GoalStatusArray;
import actionlib_msgs.GoalID; import actionlib_msgs.GoalID;
import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;
/** /**
* Client implementation for actionlib. * Client implementation for actionlib.
...@@ -35,7 +40,7 @@ import actionlib_msgs.GoalID; ...@@ -35,7 +40,7 @@ import actionlib_msgs.GoalID;
*/ */
public class ActionClient<T_ACTION_GOAL extends Message, public class ActionClient<T_ACTION_GOAL extends Message,
T_ACTION_FEEDBACK extends Message, T_ACTION_FEEDBACK extends Message,
T_ACTION_RESULT extends Message> { T_ACTION_RESULT extends Message> extends DefaultSubscriberListener {
T_ACTION_GOAL actionGoal; T_ACTION_GOAL actionGoal;
String actionGoalType; String actionGoalType;
...@@ -50,6 +55,10 @@ public class ActionClient<T_ACTION_GOAL extends Message, ...@@ -50,6 +55,10 @@ public class ActionClient<T_ACTION_GOAL extends Message,
String actionName; String actionName;
ActionClientListener callbackTarget = null; ActionClientListener callbackTarget = null;
GoalIDGenerator goalIdGenerator = 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. * Constructor for an ActionClient object.
...@@ -188,6 +197,9 @@ public class ActionClient<T_ACTION_GOAL extends Message, ...@@ -188,6 +197,9 @@ public class ActionClient<T_ACTION_GOAL extends Message,
serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType); serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType);
serverStatus = node.newSubscriber(actionName + "/status", GoalStatusArray._TYPE); serverStatus = node.newSubscriber(actionName + "/status", GoalStatusArray._TYPE);
serverFeedback.addSubscriberListener(this);
serverResult.addSubscriberListener(this);
serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() { serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() {
@Override @Override
public void onNewMessage(T_ACTION_FEEDBACK message) { public void onNewMessage(T_ACTION_FEEDBACK message) {
...@@ -258,6 +270,7 @@ public class ActionClient<T_ACTION_GOAL extends Message, ...@@ -258,6 +270,7 @@ public class ActionClient<T_ACTION_GOAL extends Message,
* @see actionlib_msgs.GoalStatusArray * @see actionlib_msgs.GoalStatusArray
*/ */
public void gotStatus(GoalStatusArray message) { public void gotStatus(GoalStatusArray message) {
statusReceivedFlag = true;
// Propagate the callback // Propagate the callback
if (callbackTarget != null) { if (callbackTarget != null) {
callbackTarget.statusReceived(message); callbackTarget.statusReceived(message);
...@@ -273,6 +286,36 @@ public class ActionClient<T_ACTION_GOAL extends Message, ...@@ -273,6 +286,36 @@ public class ActionClient<T_ACTION_GOAL extends Message,
subscribeToServer(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. * Finish the action client. Unregister publishers and listeners.
*/ */
......
...@@ -55,6 +55,10 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener ...@@ -55,6 +55,10 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
// Attach listener for the callbacks // Attach listener for the callbacks
ac.attachListener(this); ac.attachListener(this);
System.out.println("Waiting for actionlib server to start...");
ac.waitForActionServerToStart();
System.out.println("actionlib server started.");
// Create Fibonacci goal message // Create Fibonacci goal message
//goalMessage = (FibonacciActionGoal)ac.newGoalMessage(); //goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
//FibonacciGoal fibonacciGoal = goalMessage.getGoal(); //FibonacciGoal fibonacciGoal = goalMessage.getGoal();
...@@ -63,10 +67,10 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener ...@@ -63,10 +67,10 @@ public class TestClient extends AbstractNodeMain implements ActionClientListener
//fibonacciGoal.setOrder(6); //fibonacciGoal.setOrder(6);
for (i = 0; i < repeat; i++) { for (i = 0; i < repeat; i++) {
sleep(10000); //sleep(10000);
System.out.println("Sending goal #" + i + "..."); System.out.println("Sending goal #" + i + "...");
goalMessage = (FibonacciActionGoal)ac.newGoalMessage(); goalMessage = (FibonacciActionGoal)ac.newGoalMessage();
goalMessage.getGoal().setOrder(i); goalMessage.getGoal().setOrder(i*3);
ac.sendGoal(goalMessage, goalId + i); ac.sendGoal(goalMessage, goalId + i);
System.out.println("Goal sent."); System.out.println("Goal sent.");
resultReceived = false; resultReceived = false;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment