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 db586149532251cd403cd4b8d1975d56c725e622..2ee6fa57972d9ade777007d85001ebe9d6d582e8 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 680c731ddcecaa5fbeff3963e632f7976a8d19ee..fec5aca1616d559a51d2f6e02d2b802d42e3c23e 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 9851afa2708722c22c28ae618c184c5f7d86138c..08db54b0344e218615e16cdd174d9e91e5041048 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) { + ; + } } }