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

Added callback for feedback message.

parent 4e616144
No related branches found
No related tags found
No related merge requests found
......@@ -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.
*/
......
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);
}
......@@ -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) {
;
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment