From e35e5412c49cc547d38cf5283d0f8c7f45820139 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 9 Apr 2021 18:27:32 -0300 Subject: [PATCH 1/2] Add ACCEPT_AND_EXECUTE/ACCEPT_AND_DEFER to the result of the goal callback, transition from accepted to executing if ACCEPT_AND_EXECUTE was returned Signed-off-by: Ivan Santiago Paunovic --- .../ros2/rcljava/action/ActionServerGoalHandle.java | 7 +++++++ .../java/org/ros2/rcljava/action/ActionServerImpl.java | 10 ++++++---- .../java/org/ros2/rcljava/action/GoalCallback.java | 3 ++- rcljava/src/main/java/org/ros2/rcljava/node/Node.java | 2 +- .../java/org/ros2/rcljava/action/ActionServerTest.java | 3 +-- 5 files changed, 17 insertions(+), 8 deletions(-) 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..50a00c18 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -41,14 +41,13 @@ class MockGoalCallback implements GoalCallback { public ActionServerGoalHandle goalHandle; public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { - this.goalHandle = goalHandle; return CancelResponse.ACCEPT; } } From 085f579064958cc2ca75a066a742240e7d15ad00 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 13 Apr 2021 10:45:12 -0300 Subject: [PATCH 2/2] Restore code that shouldn't have been deleted Signed-off-by: Ivan Santiago Paunovic --- .../src/test/java/org/ros2/rcljava/action/ActionServerTest.java | 1 + 1 file changed, 1 insertion(+) 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 50a00c18..a9f5d07f 100644 --- a/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java +++ b/rcljava/src/test/java/org/ros2/rcljava/action/ActionServerTest.java @@ -48,6 +48,7 @@ public GoalResponse handleGoal(test_msgs.action.Fibonacci.SendGoalRequest goal) class MockCancelCallback implements CancelCallback { public ActionServerGoalHandle goalHandle; public CancelResponse handleCancel(ActionServerGoalHandle goalHandle) { + this.goalHandle = goalHandle; return CancelResponse.ACCEPT; } }