diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java new file mode 100644 index 0000000000000000000000000000000000000000..6d4d465f5074305a78c30a3adac0573a32460ccf --- /dev/null +++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServer.java @@ -0,0 +1,148 @@ +package com.github.ekumen.rosjava_actionlib; + +import org.ros.namespace.GraphName; +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.message.MessageListener; +import org.ros.internal.message.Message; +import java.util.concurrent.TimeUnit; +import actionlib_msgs.GoalStatusArray; +import actionlib_msgs.GoalID; + +public class ActionServer<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; + Subscriber<T_ACTION_GOAL> goalSuscriber = null; + Subscriber<GoalID> cancelSuscriber = null; + + Publisher<T_ACTION_RESULT> resultPublisher = null; + Publisher<T_ACTION_FEEDBACK> feedbackPublisher = null; + Publisher<GoalStatusArray> statusPublisher = null; + ConnectedNode node = null; + String actionName; + ActionServerListener callbackTarget = null; + + ActionServer (ConnectedNode node, String actionName, String actionGoalType, + String actionFeedbackType, String actionResultType) { + this.node = node; + this.actionName = actionName; + this.actionGoalType = actionGoalType; + this.actionFeedbackType = actionFeedbackType; + this.actionResultType = actionResultType; + + connect(node); + } + + public void attachListener(ActionServerListener target) { + callbackTarget = target; + } + + public void sendStatus(GoalStatusArray status) { + statusPublisher.publish(status); + } + + public void sendFeedback(T_ACTION_FEEDBACK feedback) { + feedbackPublisher.publish(feedback); + } + + public void sendResult(T_ACTION_RESULT result) { + resultPublisher.publish(result); + } + + private void publishServer(ConnectedNode node) { + statusPublisher = node.newPublisher(actionName + "/status", GoalStatusArray._TYPE); + feedbackPublisher = node.newPublisher(actionName + "/feedback", actionFeedbackType); + resultPublisher = node.newPublisher(actionName + "/result", actionResultType); + } + + private void unpublishServer() { + if (statusPublisher != null) { + statusPublisher.shutdown(5, TimeUnit.SECONDS); + statusPublisher = null; + } + if (feedbackPublisher != null) { + feedbackPublisher.shutdown(5, TimeUnit.SECONDS); + feedbackPublisher = null; + } + if (resultPublisher != null) { + resultPublisher.shutdown(5, TimeUnit.SECONDS); + resultPublisher = null; + } + } + + /*public T_ACTION_GOAL newGoalMessage() { + return goalPublisher.newMessage(); + }*/ + + private void subscribeToClient(ConnectedNode node) { + goalSuscriber = node.newSubscriber(actionName + "/goal", actionGoalType); + cancelSuscriber = node.newSubscriber(actionName + "/cancel", GoalID._TYPE); + + goalSuscriber.addMessageListener(new MessageListener<T_ACTION_GOAL>() { + @Override + public void onNewMessage(T_ACTION_GOAL message) { + gotGoal(message); + } + }); + + cancelSuscriber.addMessageListener(new MessageListener<GoalID>() { + @Override + public void onNewMessage(GoalID message) { + gotCancel(message); + } + }); + } + + private void unsubscribeToClient() { + if (goalSuscriber != null) { + goalSuscriber.shutdown(5, TimeUnit.SECONDS); + goalSuscriber = null; + } + if (cancelSuscriber != null) { + cancelSuscriber.shutdown(5, TimeUnit.SECONDS); + cancelSuscriber = null; + } + } + + public void gotGoal(T_ACTION_GOAL message) { + // Propagate the callback + if (callbackTarget != null) { + callbackTarget.goalReceived(message); + } + } + + public void gotCancel(GoalID message) { + // Propagate the callback + if (callbackTarget != null) { + callbackTarget.cancelReceived(message); + } + } + + /** + * Publishes the server's topics and suscribes to the client's topics. + */ + private void connect(ConnectedNode node) { + publishServer(node); + subscribeToClient(node); + } + + /** + * Finish the action server. Unregister publishers and listeners. + */ + public void finish() { + callbackTarget = null; + unpublishServer(); + unsubscribeToClient(); + } + + protected void finalize() { + finish(); + } +} diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServerListener.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServerListener.java new file mode 100644 index 0000000000000000000000000000000000000000..21c26aa88b5660fdff42d67b00d097d14045c44c --- /dev/null +++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/ActionServerListener.java @@ -0,0 +1,15 @@ +package com.github.ekumen.rosjava_actionlib; + +import org.ros.internal.message.Message; +import actionlib_msgs.GoalID; + + +/** + * Listener interface to receive the incoming messages from the ActionLib client. + * A server should implement this interface if it wants to receive the callbacks + * with information from the client. + */ +public interface ActionServerListener<T_ACTION_GOAL extends Message> { + void goalReceived(T_ACTION_GOAL goal); + void cancelReceived(GoalID id); +}