diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java index aedf0dbe..11117437 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerGoalHandle.java @@ -40,6 +40,13 @@ public interface ActionServerGoalHandle extends Disp */ public boolean isCanceling(); + /** + * Transition the goal to the EXECUTING state. + * + * Pre-condition: the goal must be in the ACCEPTED state. + */ + public void execute(); + /** * Transition the goal to the SUCCEEDED state. * diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java index 8c883f25..b7e740c8 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/ActionServerImpl.java @@ -322,7 +322,8 @@ private ActionServerGoalHandle executeGoalRequest( // Call user callback GoalCallback.GoalResponse response = this.goalCallback.handleGoal(requestMessage); - boolean accepted = GoalCallback.GoalResponse.ACCEPT == response; + boolean accepted = GoalCallback.GoalResponse.ACCEPT_AND_DEFER == response + || GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response; responseMessage.accept(accepted); System.out.println("Goal request handled " + accepted); @@ -334,6 +335,10 @@ private ActionServerGoalHandle executeGoalRequest( GoalHandleImpl goalHandle = this.new GoalHandleImpl( this, goalInfo, requestMessage.getGoal()); this.goalHandles.put(requestMessage.getGoalUuid(), goalHandle); + if (GoalCallback.GoalResponse.ACCEPT_AND_EXECUTE == response) { + goalHandle.execute(); + this.acceptedCallback.accept(goalHandle); + } return goalHandle; } @@ -467,9 +472,6 @@ public void execute() { this.handle, rmwRequestId, responseFromJavaConverterHandle, responseToJavaConverterHandle, responseDestructorHandle, responseMessage); - if (goalHandle != null) { - this.acceptedCallback.accept(goalHandle); - } } } } diff --git a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java index afd017b0..a448ac82 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java +++ b/rcljava/src/main/java/org/ros2/rcljava/action/GoalCallback.java @@ -20,7 +20,8 @@ public interface GoalCallback { public enum GoalResponse { REJECT, - ACCEPT, + ACCEPT_AND_EXECUTE, + ACCEPT_AND_DEFER, }; /** diff --git a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java index 63807a81..d1ba5d03 100644 --- a/rcljava/src/main/java/org/ros2/rcljava/node/Node.java +++ b/rcljava/src/main/java/org/ros2/rcljava/node/Node.java @@ -152,7 +152,7 @@ Client createClient(final Class serviceType, * @param goalCallback The callback that will be called when the @{link ActionServer} * receives a new goal request. * @param cancelCallback The callback that will be called when the @{link ActionServer} - * receives a cancle request for an active goal. + * receives a cancel request for an active goal. * @param acceptedCallback The callback that will be called when the @{link ActionServer} * accepts a goal request. */ diff --git a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java index 36f004b9..a9f5d07f 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -41,7 +41,7 @@ class MockGoalCallback implements GoalCallback