From 9c77e29e7fe7f1bdf79a7b714d449613a0ea1f3c Mon Sep 17 00:00:00 2001 From: Ernesto Corbellini <ecorbellini@ekumenlabs.com> Date: Fri, 11 Dec 2015 13:56:26 -0300 Subject: [PATCH] Added result and feedback callbacks to the client. --- .../rosjava_actionlib/ActionClient.java | 82 +++++++++++++------ .../ActionClientListener.java | 8 +- .../ekumen/rosjava_actionlib/TestClient.java | 41 +++++++++- 3 files changed, 100 insertions(+), 31 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 d18d14f..22642d2 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,31 +9,44 @@ import org.ros.node.topic.Publisher; import org.ros.message.MessageListener; import org.ros.internal.message.Message; -public class ActionClient<T_ACTION_GOAL extends Message> { +public class ActionClient<T_ACTION_GOAL extends Message, + T_ACTION_FEEDBACK extends Message, + T_ACTION_RESULT extends Message> { + T_ACTION_GOAL actionGoal; String actionGoalType; + String actionResultType; + String actionFeedbackType; Publisher<T_ACTION_GOAL> goalPublisher; //Publisher<actionlib_msgs.cancel> clientCancel; //Suscriber<actionlib_msgs.status> serverStatus; - Subscriber<actionlib_tutorials.FibonacciActionResult> serverResult; - //Suscriber<actionlib_tutorials.FibonacciActionFeedback> serverFeedback; + Subscriber<T_ACTION_RESULT> serverResult; + Subscriber<T_ACTION_FEEDBACK> serverFeedback; ConnectedNode node; String actionName; + ActionClientListener callbackTarget = null; - ActionClient (ConnectedNode node, String actionName, String actionGoalType) { + ActionClient (ConnectedNode node, String actionName, String actionGoalType, + String actionFeedbackType, String actionResultType) { this.node = node; this.actionName = actionName; this.actionGoalType = actionGoalType; - publishClient(node); + this.actionFeedbackType = actionFeedbackType; + this.actionResultType = actionResultType; + + connect(node); + } + + public void attachListener(ActionClientListener target) { + callbackTarget = target; } - void sendGoal(T_ACTION_GOAL goal) { + public void sendGoal(T_ACTION_GOAL goal) { goalPublisher.publish(goal); } private void publishClient(ConnectedNode node) { - goalPublisher = node.newPublisher(actionName + "/goal", - actionGoalType); + goalPublisher = node.newPublisher(actionName + "/goal", actionGoalType); //clientCancel = connectedNode.newPublisher("fibonacci/cancel", // actionlib_msgs.cancel._TYPE); } @@ -42,13 +55,27 @@ public class ActionClient<T_ACTION_GOAL extends Message> { return goalPublisher.newMessage(); } - private void suscribeServer(ConnectedNode node) { - serverResult = node.newSubscriber("fibonacci/result", - actionlib_tutorials.FibonacciActionResult._TYPE); + private void subscribeToServer(ConnectedNode node) { + serverResult = node.newSubscriber(actionName + "/result", actionResultType); + serverFeedback = node.newSubscriber(actionName + "/feedback", actionFeedbackType); - serverResult.addMessageListener(new MessageListener<actionlib_tutorials.FibonacciActionResult>() { + serverFeedback.addMessageListener(new MessageListener<T_ACTION_FEEDBACK>() { @Override - public void onNewMessage(actionlib_tutorials.FibonacciActionResult message) { + public void onNewMessage(T_ACTION_FEEDBACK message) { + gotFeedback(message); + } + }); + + serverResult.addMessageListener(new MessageListener<T_ACTION_RESULT>() { + @Override + public void onNewMessage(T_ACTION_RESULT message) { + gotResult(message); + } + }); + + serverResult.addMessageListener(new MessageListener<T_ACTION_RESULT>() { + @Override + public void onNewMessage(T_ACTION_RESULT message) { gotResult(message); } }); @@ -59,23 +86,26 @@ public class ActionClient<T_ACTION_GOAL extends Message> { actionlib_tutorials.FibonacciActionFeedback._TYPE);*/ } - public void gotResult(actionlib_tutorials.FibonacciActionResult message) { - actionlib_tutorials.FibonacciResult result = message.getResult(); - int[] sequence = result.getSequence(); - int i; + public void gotResult(T_ACTION_RESULT message) { + // Propagate the callback + if (callbackTarget != null) { + callbackTarget.resultReceived(message); + } + } - System.out.print("Got Fibonacci result sequence! "); - for (i=0; i<sequence.length; i++) - System.out.print(Integer.toString(sequence[i]) + " "); - System.out.print("\n"); + public void gotFeedback(T_ACTION_FEEDBACK message) { + // Propagate the callback + if (callbackTarget != null) { + callbackTarget.feedbackReceived(message); + } } -/** - * Publishes the client's topics and suscribes to the server's topics. - */ - public void connect(ConnectedNode node) { + /** + * Publishes the client's topics and suscribes to the server's topics. + */ + private void connect(ConnectedNode node) { publishClient(node); - //suscribeServer(node); + subscribeToServer(node); } } 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 c3453a8..680c731 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 @@ -4,9 +4,11 @@ import org.ros.internal.message.Message; /** * Listener interface to receive the incoming messages from the ActionLib server. + * A client should implement this interface if it wants to receive the callbacks + * with information from the server. */ -public interface ActionClientListener { - void resultReceived(Message result); - void feedbackReceived(Message feedback); +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); } 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 333677e..9851afa 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 @@ -3,10 +3,15 @@ package com.github.ekumen.rosjava_actionlib; import org.ros.namespace.GraphName; import org.ros.node.AbstractNodeMain; import org.ros.node.ConnectedNode; +import org.ros.internal.message.Message; import actionlib_tutorials.FibonacciActionGoal; +import actionlib_tutorials.FibonacciActionFeedback; +import actionlib_tutorials.FibonacciActionResult; import actionlib_tutorials.FibonacciGoal; +import actionlib_tutorials.FibonacciFeedback; +import actionlib_tutorials.FibonacciResult; -public class TestClient extends AbstractNodeMain { +public class TestClient extends AbstractNodeMain implements ActionClientListener<FibonacciActionFeedback, FibonacciActionResult> { ActionClient ac; @Override @@ -16,7 +21,10 @@ public class TestClient extends AbstractNodeMain { @Override public void onStart(ConnectedNode node) { - ac = new ActionClient<FibonacciActionGoal>(node, "/fibonacci", FibonacciActionGoal._TYPE); + ac = new ActionClient<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE, FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE); + + // Attach listener for the callbacks + ac.attachListener(this); // publish a goal message FibonacciActionGoal goalMessage = (FibonacciActionGoal)ac.newGoalMessage(); @@ -36,4 +44,33 @@ public class TestClient extends AbstractNodeMain { } } } + + @Override + public void resultReceived(FibonacciActionResult message) { + FibonacciResult result = message.getResult(); + int[] sequence = result.getSequence(); + int i; + + System.out.print("Got Fibonacci result sequence!"); + for (i=0; i<sequence.length; i++) + System.out.print(Integer.toString(sequence[i]) + " "); + System.out.print("\n"); + } + + @Override + public void feedbackReceived(FibonacciActionFeedback message) { + FibonacciFeedback result = message.getFeedback(); + int[] sequence = result.getSequence(); + int i; + + System.out.print("Feedback from Fibonacci server: "); + for (i=0; i<sequence.length; i++) + System.out.print(Integer.toString(sequence[i]) + " "); + System.out.print("\n"); + } + + @Override + public void statusReceived(Message status) { + + } } -- GitLab