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 ef72bb184625fee546dfcab4ed665589d9a6ebc7..010bb7d1c3a3ac36692a822756432f827a7712b2 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 @@ -11,9 +11,11 @@ import java.util.concurrent.TimeUnit; import java.util.Timer; import java.util.TimerTask; import java.util.HashMap; +import java.util.Vector; import java.lang.reflect.Method; import actionlib_msgs.GoalStatusArray; import actionlib_msgs.GoalID; +import actionlib_msgs.GoalStatus; public class ActionServer<T_ACTION_GOAL extends Message, T_ACTION_FEEDBACK extends Message, @@ -99,6 +101,10 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } + /** + * Subscribe to the action client's topics: goal and cancel. + * @param node The ROS node connected to the master. + */ private void subscribeToClient(ConnectedNode node) { goalSuscriber = node.newSubscriber(actionName + "/goal", actionGoalType); cancelSuscriber = node.newSubscriber(actionName + "/cancel", GoalID._TYPE); @@ -118,6 +124,9 @@ public class ActionServer<T_ACTION_GOAL extends Message, }); } + /** + * Unsubscribe from the client's topics. + */ private void unsubscribeToClient() { if (goalSuscriber != null) { goalSuscriber.shutdown(5, TimeUnit.SECONDS); @@ -129,14 +138,44 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } + /** + * Called when we get a message on the subscribed goal topic. + */ public void gotGoal(T_ACTION_GOAL goal) { - goalTracker.put(getGoalId(goal).getId(), new ServerGoal(goal)); + boolean accepted = false; + String goalIdString = getGoalId(goal).getId(); + + // start tracking this newly received goal + goalTracker.put(goalIdString, new ServerGoal(goal)); // Propagate the callback if (callbackTarget != null) { + // inform the user of a received message callbackTarget.goalReceived(goal); + // ask the user to accept the goal + accepted = callbackTarget.acceptGoal(goal); + if (accepted) { + // the user accepted the goal + try { + goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.ACCEPT); + } + catch (Exception e) { + e.printStackTrace(System.out); + } + } else { + // the user rejected the goal + try { + goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.REJECT); + } + catch (Exception e) { + e.printStackTrace(System.out); + } + } } } + /** + * Called when we get a message on the subscribed cancel topic. + */ public void gotCancel(GoalID gid) { // Propagate the callback if (callbackTarget != null) { @@ -144,8 +183,22 @@ public class ActionServer<T_ACTION_GOAL extends Message, } } + /** + * Publishes the current status on the server's status topic. + * This is used like a heartbeat to update the status of every tracked goal. + */ public void sendStatusTick() { GoalStatusArray status = statusPublisher.newMessage(); + GoalStatus goalStatus; + Vector<GoalStatus> goalStatusList = new Vector<GoalStatus>(); + + for (ServerGoal sg : goalTracker.values()) { + goalStatus = node.getTopicMessageFactory().newFromType(GoalStatus._TYPE); + goalStatus.setGoalId(getGoalId(sg.goal)); + goalStatus.setStatus((byte)sg.state.getState()); + goalStatusList.add(goalStatus); + } + status.setStatusList(goalStatusList); sendStatus(status); } @@ -157,6 +210,11 @@ public class ActionServer<T_ACTION_GOAL extends Message, return feedbackPublisher.newMessage(); } + /** + * Returns the goal ID object related to a given action goal. + * @param goal An action goal message. + * @return The goal ID object. + */ public GoalID getGoalId(T_ACTION_GOAL goal) { GoalID gid = null; try { @@ -170,6 +228,34 @@ public class ActionServer<T_ACTION_GOAL extends Message, return gid; } + /** + * Get the current state of the referenced goal. + * @param goalId String representing the ID of the goal. + * @return The current state of the goal or -100 if the goal ID is not tracked. + * @see actionlib_msgs.GoalStatus + */ + public int getGoalState(String goalId) { + int ret = 0; + + if (goalTracker.containsKey(goalId)) { + ret = goalTracker.get(goalId).state.getState(); + } else { + ret = -100; + } + return ret; + } + + /** + * Express a succeed event for this goal. The state of the goal will be updated. + */ + public void setSucceed(String goalIdString) { + try { + goalTracker.get(goalIdString).state.transition(ServerStateMachine.Events.SUCCEED); + } + catch (Exception e) { + } + } + /** * Publishes the server's topics and suscribes to the client's topics. */ 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 index 21c26aa88b5660fdff42d67b00d097d14045c44c..80b4e19cf834994d261c448746eb0d9f98ca7898 100644 --- 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 @@ -10,6 +10,25 @@ import actionlib_msgs.GoalID; * with information from the client. */ public interface ActionServerListener<T_ACTION_GOAL extends Message> { + /** + * This callback is called when a message is received on the goal topic. + * Note: this method is called right after the server starts tracking this + * goal and is intended for informative purposes. + * @param goal the action goal received. + */ void goalReceived(T_ACTION_GOAL goal); + + /** + * This callback is called when a message is received on the cancel topic. + * @param id Goal ID object that was received. + */ void cancelReceived(GoalID id); + + /** + * Callback method to accept a recently received action goal. + * @param goal The action goal received. + * @return The implementer must return true if he accepts the goal or false + * otherwise. + */ + boolean acceptGoal(T_ACTION_GOAL goal); } diff --git a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestServer.java b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestServer.java index 5cf9870b783a5d2f519132f9a99d3231f4b3b515..e74172c0c2e4eb43b19c64d1c6ad31b9ceab8914 100644 --- a/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestServer.java +++ b/src/rosjava_actionlib/rosjava_actionlib/src/main/java/com/github/ekumen/rosjava_actionlib/TestServer.java @@ -17,6 +17,7 @@ import actionlib_msgs.GoalStatus; public class TestServer extends AbstractNodeMain implements ActionServerListener<FibonacciActionGoal> { private ActionServer<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult> as = null; + private volatile FibonacciActionGoal currentGoal = null; @Override public GraphName getDefaultNodeName() { @@ -25,21 +26,74 @@ public class TestServer extends AbstractNodeMain implements ActionServerListener @Override public void onStart(ConnectedNode node) { + FibonacciActionResult result; + as = new ActionServer<FibonacciActionGoal, FibonacciActionFeedback, FibonacciActionResult>(node, "/fibonacci", FibonacciActionGoal._TYPE, FibonacciActionFeedback._TYPE, FibonacciActionResult._TYPE); as.attachListener(this); + + while(true) { + if (currentGoal != null) { + result = as.newResultMessage(); + result.getResult().setSequence(fibonacciSequence(currentGoal.getGoal().getOrder())); + as.setSucceed(currentGoal.getGoalId().getId()); + as.sendResult(result); + currentGoal = null; + } + } } @Override public void goalReceived(FibonacciActionGoal goal) { System.out.println("Goal received."); - as.sendResult(as.newResultMessage()); + sleep(2000); + System.out.println("Sending result..."); + sleep(2000);as.sendResult(as.newResultMessage()); } @Override public void cancelReceived(GoalID id) { System.out.println("Cancel received."); } + + @Override + public boolean acceptGoal(FibonacciActionGoal goal) { + // If we don't have a goal, accept it. Otherwise, reject it. + if (currentGoal == null) { + currentGoal = goal; + System.out.println("Goal accepted."); + return true; + } else { + System.out.println("We already have a goal! New goal reject."); + return false; + } + } + + private int[] fibonacciSequence(int order) { + int i; + int[] fib = new int[order + 2]; + + fib[0] = 0; + fib[1] = 1; + + for (i = 2; i < (order + 2); i++) { + fib[i] = fib[i - 1] + fib[i - 2]; + } + return fib; + } + + /* + * Sleep for an amount on miliseconds. + * @param msec Number or miliseconds to sleep. + */ + private void sleep(long msec) { + try { + Thread.sleep(msec); + } + catch (InterruptedException ex) { + } + } + }