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 index 6d4d465f5074305a78c30a3adac0573a32460ccf..3b1d9f0ce4d3486fd676d7b8176798c152ba6c73 100644 --- 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 @@ -1,4 +1,4 @@ -package com.github.ekumen.rosjava_actionlib; + package com.github.ekumen.rosjava_actionlib; import org.ros.namespace.GraphName; import org.ros.node.AbstractNodeMain; @@ -8,6 +8,8 @@ import org.ros.node.topic.Publisher; import org.ros.message.MessageListener; import org.ros.internal.message.Message; import java.util.concurrent.TimeUnit; +import java.util.Timer; +import java.util.TimerTask; import actionlib_msgs.GoalStatusArray; import actionlib_msgs.GoalID; @@ -15,19 +17,20 @@ 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; + private T_ACTION_GOAL actionGoal; + private String actionGoalType; + private String actionResultType; + private String actionFeedbackType; + private Subscriber<T_ACTION_GOAL> goalSuscriber = null; + private Subscriber<GoalID> cancelSuscriber = null; + + private Publisher<T_ACTION_RESULT> resultPublisher = null; + private Publisher<T_ACTION_FEEDBACK> feedbackPublisher = null; + private Publisher<GoalStatusArray> statusPublisher = null; + private ConnectedNode node = null; + private String actionName; + private ActionServerListener callbackTarget = null; + private Timer statusTick = new Timer(); ActionServer (ConnectedNode node, String actionName, String actionGoalType, String actionFeedbackType, String actionResultType) { @@ -60,6 +63,12 @@ public class ActionServer<T_ACTION_GOAL extends Message, statusPublisher = node.newPublisher(actionName + "/status", GoalStatusArray._TYPE); feedbackPublisher = node.newPublisher(actionName + "/feedback", actionFeedbackType); resultPublisher = node.newPublisher(actionName + "/result", actionResultType); + statusTick.scheduleAtFixedRate(new TimerTask() { + @Override + public void run() { + sendStatusTick(); + } + }, 2000, 1000); } private void unpublishServer() { @@ -77,10 +86,6 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } - /*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); @@ -125,6 +130,19 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } + public void sendStatusTick() { + GoalStatusArray status = statusPublisher.newMessage(); + sendStatus(status); + } + + public T_ACTION_RESULT newResultMessage() { + return resultPublisher.newMessage(); + } + + public T_ACTION_FEEDBACK newFeedbackMessage() { + return feedbackPublisher.newMessage(); + } + /** * Publishes the server's topics and suscribes to the client's topics. */